{
 "cells": [
  {
   "cell_type": "code",
   "execution_count": 1,
   "metadata": {},
   "outputs": [],
   "source": [
    "from sympy.physics.mechanics import dynamicsymbols, inertia\n",
    "import sympy as sp, numpy as np\n",
    "import sympy.physics.vector as vector\n",
    "from sympy.algebras.quaternion import Quaternion\n",
    "from sympy.utilities.lambdify import lambdastr\n",
    "from sympy import symbols\n",
    "import sys, os\n",
    "\n",
    "from simupy import codegen\n",
    "from simupy.matrices import construct_explicit_matrix\n",
    "\n",
    "INCLUDE_NUTATION_PRECESSION = False # below is a prototype derivation of the equations of motion including nutation and precession but it is not currently supported.\n",
    "\n",
    "def display_dict(d):\n",
    "    for k,v in d.items():\n",
    "        display(sp.Eq(k,v))"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# Kinematics Problem setup\n",
    "\n",
    "\n",
    "## Symbol Table\n",
    "\n",
    "Symbols are defined in this cell with their definitions neary by in in-line comments or through explicit naming.\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 2,
   "metadata": {},
   "outputs": [],
   "source": [
    "t = dynamicsymbols._t # time symbol\n",
    "\n",
    "omega_p = symbols('omega_p') # planet rotation angular velocity\n",
    "\n",
    "if INCLUDE_NUTATION_PRECESSION:\n",
    "    # pci2pcf_rotation_matrix = construct_explicit_matrix('pci2pcf', 3,3)\n",
    "    pci2pcf_rotation_matrix = sp.MatrixSymbol('pci2pcf', 3,3)\n",
    "\n",
    "lamda_D, phi_D, h_D = dynamicsymbols('lamda_D phi_D h_D') \n",
    "# planetodetic longitude, planetodetic latitude, planetodetic altitude\n",
    "# order selected to math erfa's gc2gd signature\n",
    "# symbol choice to match NESC's \"Fundamentals of Geodetic Kinematics\"\n",
    "# swapping _E (earth) to _D for generic planetoDetic\n",
    "\n",
    "\n",
    "a, f = symbols('a f', real=True, nonnegative=True) # planet equitorial radius and flattening coefficient\n",
    "\n",
    "# placeholder derivatives for local acceleration trimming\n",
    "lamda_D_dot, phi_D_dot, h_D_dot = dynamicsymbols('lamdadot_D phidot_D hdot_D')\n",
    "\n",
    "planetodetic_pos_derivs = {}\n",
    "for orig, dotted in zip((lamda_D, phi_D, h_D,),\n",
    "                        (lamda_D_dot, phi_D_dot)):\n",
    "    planetodetic_pos_derivs[orig.diff(t)] = dotted\n",
    "    \n",
    "\n",
    "psi, theta, phi = dynamicsymbols('psi theta phi') # euler angles local to body : yaw pitch roll\n",
    "\n",
    "\n",
    "V_N, V_E, V_D = V_NED = sp.Array(\n",
    "    # relative velocity in local frame (NED) -- add winds to get aero\n",
    "    dynamicsymbols('V_N V_E V_D')\n",
    ")\n",
    "\n",
    "W_N, W_E, W_D = W_NED = sp.Array(\n",
    "    dynamicsymbols('W_N W_E W_D')\n",
    ")\n",
    "\n",
    "# inertial position, velocity, and acceleration of vehicle center of mass\n",
    "# expressed in PCI frame\n",
    "px, py, pz = inertial_pos_I = sp.Array(dynamicsymbols('p_x p_y p_z'))\n",
    "vx, vy, vz = inertial_vel_I = sp.Array(dynamicsymbols('v_x v_y v_z'))\n",
    "ax, ay, az = inertial_acc_I = sp.Array(dynamicsymbols('a_x a_y a_z'))\n",
    "\n",
    "\n",
    "\n",
    "# planet fixed rectangular position coordinates\n",
    "planet_fixed_position_x, planet_fixed_position_y, planet_fixed_position_z = planet_fixed_position = sp.Array(\n",
    "    dynamicsymbols('ap_x ap_y ap_z')\n",
    ")\n",
    "\n",
    "planet_fixed_func_args = (t, lamda_D, phi_D, h_D)\n",
    "\n",
    "# atmospheric properties\n",
    "density, speed_of_sound, viscosity = dynamicsymbols('rho c_s mu')\n",
    "\n",
    "# translational acceleration due to gravity\n",
    "gx, gy, gz = g = sp.Array(dynamicsymbols('g_x g_y g_z'))\n",
    "\n",
    "\n",
    "# non-unit quaternion component symbols to represent I to body B attitude\n",
    "q = sp.Array(\n",
    "    dynamicsymbols('q_0:4') \n",
    ")\n",
    "qmag = sp.sqrt(sp.tensorcontraction(sp.tensorproduct(q, q), (0,1))) # magnitude of non-unit quaternion\n",
    "qq = Quaternion(*(q/qmag)) # q as unit-quaternion Quaternion object\n",
    "cq = dynamicsymbols('c_q') # scalar control variable for derivative of q. See Rucker 2018\n",
    "\n",
    "# angular velocity of vehicle body B w.r.t. inertial in body-fixed coordinates\n",
    "omega_X, omega_Y, omega_Z = omega = I_ang_vel_B = sp.Array(dynamicsymbols('omega_X omega_Y omega_Z'))\n",
    "\n",
    "# angular acceleration of vehicle body B w.r.t. inertial in body-fixed coordinates\n",
    "alpha_X, alpha_Y, alpha_Z = I_ang_acc_B = sp.Array(dynamicsymbols('alpha_X alpha_Y alpha_Z'))\n",
    "\n",
    "# inertial translational acceleration EXCLUDING GRAVITY of vehicle expressed in body-fixed coordinates\n",
    "# intended to capture vehicle dynamics based on vehicle configuration (aero, prop, other control effectors, etc)\n",
    "# input to kinematics block\n",
    "AX, AY, AZ = inertial_acc_B = sp.Array(dynamicsymbols('A_X A_Y A_Z'))\n",
    "\n",
    "airspeed_in_B = sp.Array(dynamicsymbols('u_B v_B w_B')) # true airspeed components in body-fixed coordinates\n",
    "V_T, alpha, beta = sp.Array(dynamicsymbols('V_T alpha beta')) # true airspeed magnitude, angles of attack and sideslip\n",
    "NED_ang_vel_B = sp.Array(dynamicsymbols('p_B q_B r_B')) # components of angular velocity of the body in air in body-fixed coordinate system\n",
    "\n",
    "kinematics_state = sp.Array((*inertial_pos_I, *q, *inertial_vel_I, *I_ang_vel_B))\n",
    "kinematics_input = sp.Array((*inertial_acc_B, *I_ang_acc_B, cq))\n",
    "\n",
    "kinematics_output = sp.Array((\n",
    "   *kinematics_state, \n",
    "\n",
    "   # these are guidance and control feedback outputs, essentially\n",
    "   lamda_D, phi_D, h_D, # planet-fixed planetodetic spherical position coordinates -- follow logic of planet_fixed_position -- can be used for navigation/guidance problems\n",
    "   psi, theta, phi, # NED-to-body yaw pitch roll, if possible\n",
    "\n",
    "   # what is needed for aerodynamics \n",
    "   density, speed_of_sound, viscosity, # speed of sound, viscosity, and true airspeed give the needed atmospheric conditions\n",
    "   V_T, alpha, beta, # true air speed, angles of attack and sideslip\n",
    "   *NED_ang_vel_B, # components of angular velocity of the body in air in body-fixed coordinate system for computing aerodynamic damping\n",
    "\n",
    "   *V_NED, # planet-fixed velocities in local frame\n",
    "   *W_NED, # local wind\n",
    "))\n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Symbolic function placeholders\n",
    "\n",
    "These symbols represent numerical functions to assign particular variables in the codegeneration. This approach is used to allow for parameterized models and to take advantage of numerically efficient algorithms when appropriate, such as for the planetodetic problem."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 3,
   "metadata": {},
   "outputs": [],
   "source": [
    "kinematics_function_modules = {} # defines the namespace of the functions used in the Planet\n",
    "\n",
    "# symbolic placeholder for planet centered fixed to planetodetic coordinates and back\n",
    "if INCLUDE_NUTATION_PRECESSION:\n",
    "    pci2pcf_rotation_matrix_function = symbols('pci2pcf', cls=sp.Function)(t)\n",
    "    pci2pcf_rotation_matrix_function.shape = (3,3)\n",
    "    kinematics_function_modules[pci2pcf_rotation_matrix_function] = 'self'\n",
    "    \n",
    "    pcf2pd = symbols('planetodetics.pcf2pd', cls=sp.Function)(*planet_fixed_position)\n",
    "else:\n",
    "    pcf2pd = symbols('planetodetics.pcf2pd', cls=sp.Function)(*inertial_pos_I)\n",
    "\n",
    "pcf2pd.shape = (3,)\n",
    "\n",
    "pd2pcf = symbols('planetodetics.pd2pcf', cls=sp.Function)(lamda_D, phi_D, h_D)\n",
    "pd2pcf.shape = (3,)\n",
    "\n",
    "kinematics_function_modules[pcf2pd] = 'self'\n",
    "\n",
    "\n",
    "# function for acceleration due to gravity \n",
    "if not INCLUDE_NUTATION_PRECESSION:\n",
    "    gravity_func = sp.symbols('gravity', cls=sp.Function)(*inertial_pos_I)\n",
    "else:\n",
    "    gravity_func = sp.symbols('gravity', cls=sp.Function)(*planet_fixed_position)\n",
    "gravity_func.shape = (3,)\n",
    "kinematics_function_modules[gravity_func] = 'self'\n",
    "\n",
    "# function for wind model\n",
    "winds_func = sp.symbols('winds', cls=sp.Function)(*planet_fixed_func_args)\n",
    "winds_func.shape = (3,)\n",
    "kinematics_function_modules[winds_func] = 'self'\n",
    "\n",
    "# function for atmospheric model\n",
    "atmosphere_func = symbols('atmosphere', cls=sp.Function)(*planet_fixed_func_args)\n",
    "atmosphere_func.shape = (3,)\n",
    "kinematics_function_modules[atmosphere_func] = 'self'\n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Kinematic Relationship\n",
    "\n",
    "Construct frames of reference, points, and kinematic relationships similar to Kane's method."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 4,
   "metadata": {},
   "outputs": [],
   "source": [
    "# planet centered Inertial\n",
    "I = vector.frame.ReferenceFrame('I') \n",
    "\n",
    "# Planet-fixed reference frame\n",
    "if INCLUDE_NUTATION_PRECESSION:\n",
    "    P = I.orientnew('P', 'DCM', sp.Matrix(pci2pcf_rotation_matrix), '321') # planet centered, planet fixed\n",
    "    P.set_ang_vel(I, omega_p*P.z)\n",
    "else: \n",
    "    P = I.orientnew('P', 'Body', [omega_p*t, 0, 0], '321') # planet centered, planet fixed\n",
    "\n",
    "# Local NED frame\n",
    "NED = P.orientnew('NED', 'Body', [lamda_D, -phi_D-sp.pi/2, 0], '321') # local/NED on planet surface\n",
    "\n",
    "# Define planet center as stationary\n",
    "I0 = vector.Point('I_0') # planet center\n",
    "\n",
    "\n",
    "# reference frame for vehicle body B\n",
    "B = I.orientnew('B', 'quaternion', qq.args)\n",
    "\n",
    "B.set_ang_vel(I, omega_X*B.x + omega_Y*B.y + omega_Z*B.z)\n",
    "\n",
    "\n",
    "B_cm = vector.Point('B_cm') # center of mass of vehicle body B\n",
    "\n",
    "I0.set_vel(I, 0)\n",
    "I0.set_acc(I, 0)\n",
    "I0.set_vel(P, 0)\n",
    "I0.set_acc(P, 0)\n",
    "\n",
    "B_cm.set_pos(I0, px*I.x + py*I.y + pz*I.z)\n",
    "\n",
    "B_cm.set_vel(I, vx*I.x + vy*I.y + vz*I.z)\n",
    "B_cm.set_vel(B, 0)\n",
    "B_cm.set_acc(I, ax*I.x + ay*I.y + az*I.z)\n",
    "B_cm.set_acc(B, 0)\n",
    "\n",
    "\n",
    "\n",
    "\n",
    "# components of acceleration\n",
    "translation_acceleration_due_to_gravity = gx*I.x + gy*I.y + gz*I.z # from the gravitational model\n",
    "translation_acceleration_due_to_body = AX*B.x + AY*B.y + AZ*B.z # from the dynamics model\n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# Derive Equations of Motion\n",
    "\n",
    "## Inertial translational position derivative\n",
    "\n",
    "Inertial position derivative is a simple derivative in the inertial frame."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 5,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/latex": [
       "$\\displaystyle \\frac{d}{d t} \\operatorname{p_{x}}{\\left(t \\right)} = \\operatorname{v_{x}}{\\left(t \\right)}$"
      ],
      "text/plain": [
       "Eq(Derivative(p_x(t), t), v_x(t))"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    },
    {
     "data": {
      "text/latex": [
       "$\\displaystyle \\frac{d}{d t} \\operatorname{p_{y}}{\\left(t \\right)} = \\operatorname{v_{y}}{\\left(t \\right)}$"
      ],
      "text/plain": [
       "Eq(Derivative(p_y(t), t), v_y(t))"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    },
    {
     "data": {
      "text/latex": [
       "$\\displaystyle \\frac{d}{d t} \\operatorname{p_{z}}{\\left(t \\right)} = \\operatorname{v_{z}}{\\left(t \\right)}$"
      ],
      "text/plain": [
       "Eq(Derivative(p_z(t), t), v_z(t))"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    }
   ],
   "source": [
    "inertial_pos_I_deriv_dict = sp.solve(inertial_pos_I.diff(t) - inertial_vel_I, inertial_pos_I.diff(t))\n",
    "display_dict(inertial_pos_I_deriv_dict)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Quaternion derivative\n",
    "\n",
    "The derivative for non-unit quaternion comes from [TODO: ADD CITATION]. It is straightforward from unit quaternion derivatives after a normalization. This expression includes a numerical control term $c_q$."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 6,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/latex": [
       "$\\displaystyle \\frac{d}{d t} \\operatorname{q_{0}}{\\left(t \\right)} = \\operatorname{c_{q}}{\\left(t \\right)} \\operatorname{q_{0}}{\\left(t \\right)} - \\frac{\\omega_{X}{\\left(t \\right)} \\operatorname{q_{1}}{\\left(t \\right)}}{2} - \\frac{\\omega_{Y}{\\left(t \\right)} \\operatorname{q_{2}}{\\left(t \\right)}}{2} - \\frac{\\omega_{Z}{\\left(t \\right)} \\operatorname{q_{3}}{\\left(t \\right)}}{2}$"
      ],
      "text/plain": [
       "Eq(Derivative(q_0(t), t), c_q(t)*q_0(t) - omega_X(t)*q_1(t)/2 - omega_Y(t)*q_2(t)/2 - omega_Z(t)*q_3(t)/2)"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    },
    {
     "data": {
      "text/latex": [
       "$\\displaystyle \\frac{d}{d t} \\operatorname{q_{1}}{\\left(t \\right)} = \\operatorname{c_{q}}{\\left(t \\right)} \\operatorname{q_{1}}{\\left(t \\right)} + \\frac{\\omega_{X}{\\left(t \\right)} \\operatorname{q_{0}}{\\left(t \\right)}}{2} - \\frac{\\omega_{Y}{\\left(t \\right)} \\operatorname{q_{3}}{\\left(t \\right)}}{2} + \\frac{\\omega_{Z}{\\left(t \\right)} \\operatorname{q_{2}}{\\left(t \\right)}}{2}$"
      ],
      "text/plain": [
       "Eq(Derivative(q_1(t), t), c_q(t)*q_1(t) + omega_X(t)*q_0(t)/2 - omega_Y(t)*q_3(t)/2 + omega_Z(t)*q_2(t)/2)"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    },
    {
     "data": {
      "text/latex": [
       "$\\displaystyle \\frac{d}{d t} \\operatorname{q_{2}}{\\left(t \\right)} = \\operatorname{c_{q}}{\\left(t \\right)} \\operatorname{q_{2}}{\\left(t \\right)} + \\frac{\\omega_{X}{\\left(t \\right)} \\operatorname{q_{3}}{\\left(t \\right)}}{2} + \\frac{\\omega_{Y}{\\left(t \\right)} \\operatorname{q_{0}}{\\left(t \\right)}}{2} - \\frac{\\omega_{Z}{\\left(t \\right)} \\operatorname{q_{1}}{\\left(t \\right)}}{2}$"
      ],
      "text/plain": [
       "Eq(Derivative(q_2(t), t), c_q(t)*q_2(t) + omega_X(t)*q_3(t)/2 + omega_Y(t)*q_0(t)/2 - omega_Z(t)*q_1(t)/2)"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    },
    {
     "data": {
      "text/latex": [
       "$\\displaystyle \\frac{d}{d t} \\operatorname{q_{3}}{\\left(t \\right)} = \\operatorname{c_{q}}{\\left(t \\right)} \\operatorname{q_{3}}{\\left(t \\right)} - \\frac{\\omega_{X}{\\left(t \\right)} \\operatorname{q_{2}}{\\left(t \\right)}}{2} + \\frac{\\omega_{Y}{\\left(t \\right)} \\operatorname{q_{1}}{\\left(t \\right)}}{2} + \\frac{\\omega_{Z}{\\left(t \\right)} \\operatorname{q_{0}}{\\left(t \\right)}}{2}$"
      ],
      "text/plain": [
       "Eq(Derivative(q_3(t), t), c_q(t)*q_3(t) - omega_X(t)*q_2(t)/2 + omega_Y(t)*q_1(t)/2 + omega_Z(t)*q_0(t)/2)"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    }
   ],
   "source": [
    "# quaternion kinematics\n",
    "def Omega(ang_vel):\n",
    "    return sp.Matrix(\n",
    "        [\n",
    "            [0, -ang_vel[0], -ang_vel[1], -ang_vel[2]],\n",
    "            [ang_vel[0], 0, ang_vel[2], -ang_vel[1]],\n",
    "            [ang_vel[1], -ang_vel[2], 0, ang_vel[0]],\n",
    "            [ang_vel[2], ang_vel[1], -ang_vel[0], 0]\n",
    "        ]\n",
    "    )\n",
    "\n",
    "qvec = sp.Matrix(q)\n",
    "qdot_vec_expr = Omega(I_ang_vel_B)@qvec/2 + cq*qvec\n",
    "qdotvec = sp.Matrix(q.diff(t))\n",
    "q_deriv_dict = sp.solve(qdot_vec_expr - qdotvec, qdotvec)\n",
    "\n",
    "\n",
    "display_dict(q_deriv_dict)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Inertial translational velocity derivative\n",
    "\n",
    "We assume that the `Vehicle` outputs the translational acceleration due non-gravitational forces and the gravitaty model incorporates the translational acceleration due to gravity."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 7,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/latex": [
       "$\\displaystyle \\frac{d}{d t} \\operatorname{v_{x}}{\\left(t \\right)} = \\frac{\\operatorname{A_{X}}{\\left(t \\right)} \\operatorname{q_{0}}^{2}{\\left(t \\right)} + \\operatorname{A_{X}}{\\left(t \\right)} \\operatorname{q_{1}}^{2}{\\left(t \\right)} - \\operatorname{A_{X}}{\\left(t \\right)} \\operatorname{q_{2}}^{2}{\\left(t \\right)} - \\operatorname{A_{X}}{\\left(t \\right)} \\operatorname{q_{3}}^{2}{\\left(t \\right)} - 2 \\operatorname{A_{Y}}{\\left(t \\right)} \\operatorname{q_{0}}{\\left(t \\right)} \\operatorname{q_{3}}{\\left(t \\right)} + 2 \\operatorname{A_{Y}}{\\left(t \\right)} \\operatorname{q_{1}}{\\left(t \\right)} \\operatorname{q_{2}}{\\left(t \\right)} + 2 \\operatorname{A_{Z}}{\\left(t \\right)} \\operatorname{q_{0}}{\\left(t \\right)} \\operatorname{q_{2}}{\\left(t \\right)} + 2 \\operatorname{A_{Z}}{\\left(t \\right)} \\operatorname{q_{1}}{\\left(t \\right)} \\operatorname{q_{3}}{\\left(t \\right)} + \\operatorname{g_{x}}{\\left(t \\right)} \\operatorname{q_{0}}^{2}{\\left(t \\right)} + \\operatorname{g_{x}}{\\left(t \\right)} \\operatorname{q_{1}}^{2}{\\left(t \\right)} + \\operatorname{g_{x}}{\\left(t \\right)} \\operatorname{q_{2}}^{2}{\\left(t \\right)} + \\operatorname{g_{x}}{\\left(t \\right)} \\operatorname{q_{3}}^{2}{\\left(t \\right)}}{\\operatorname{q_{0}}^{2}{\\left(t \\right)} + \\operatorname{q_{1}}^{2}{\\left(t \\right)} + \\operatorname{q_{2}}^{2}{\\left(t \\right)} + \\operatorname{q_{3}}^{2}{\\left(t \\right)}}$"
      ],
      "text/plain": [
       "Eq(Derivative(v_x(t), t), (A_X(t)*q_0(t)**2 + A_X(t)*q_1(t)**2 - A_X(t)*q_2(t)**2 - A_X(t)*q_3(t)**2 - 2*A_Y(t)*q_0(t)*q_3(t) + 2*A_Y(t)*q_1(t)*q_2(t) + 2*A_Z(t)*q_0(t)*q_2(t) + 2*A_Z(t)*q_1(t)*q_3(t) + g_x(t)*q_0(t)**2 + g_x(t)*q_1(t)**2 + g_x(t)*q_2(t)**2 + g_x(t)*q_3(t)**2)/(q_0(t)**2 + q_1(t)**2 + q_2(t)**2 + q_3(t)**2))"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    },
    {
     "data": {
      "text/latex": [
       "$\\displaystyle \\frac{d}{d t} \\operatorname{v_{y}}{\\left(t \\right)} = \\frac{2 \\operatorname{A_{X}}{\\left(t \\right)} \\operatorname{q_{0}}{\\left(t \\right)} \\operatorname{q_{3}}{\\left(t \\right)} + 2 \\operatorname{A_{X}}{\\left(t \\right)} \\operatorname{q_{1}}{\\left(t \\right)} \\operatorname{q_{2}}{\\left(t \\right)} + \\operatorname{A_{Y}}{\\left(t \\right)} \\operatorname{q_{0}}^{2}{\\left(t \\right)} - \\operatorname{A_{Y}}{\\left(t \\right)} \\operatorname{q_{1}}^{2}{\\left(t \\right)} + \\operatorname{A_{Y}}{\\left(t \\right)} \\operatorname{q_{2}}^{2}{\\left(t \\right)} - \\operatorname{A_{Y}}{\\left(t \\right)} \\operatorname{q_{3}}^{2}{\\left(t \\right)} - 2 \\operatorname{A_{Z}}{\\left(t \\right)} \\operatorname{q_{0}}{\\left(t \\right)} \\operatorname{q_{1}}{\\left(t \\right)} + 2 \\operatorname{A_{Z}}{\\left(t \\right)} \\operatorname{q_{2}}{\\left(t \\right)} \\operatorname{q_{3}}{\\left(t \\right)} + \\operatorname{g_{y}}{\\left(t \\right)} \\operatorname{q_{0}}^{2}{\\left(t \\right)} + \\operatorname{g_{y}}{\\left(t \\right)} \\operatorname{q_{1}}^{2}{\\left(t \\right)} + \\operatorname{g_{y}}{\\left(t \\right)} \\operatorname{q_{2}}^{2}{\\left(t \\right)} + \\operatorname{g_{y}}{\\left(t \\right)} \\operatorname{q_{3}}^{2}{\\left(t \\right)}}{\\operatorname{q_{0}}^{2}{\\left(t \\right)} + \\operatorname{q_{1}}^{2}{\\left(t \\right)} + \\operatorname{q_{2}}^{2}{\\left(t \\right)} + \\operatorname{q_{3}}^{2}{\\left(t \\right)}}$"
      ],
      "text/plain": [
       "Eq(Derivative(v_y(t), t), (2*A_X(t)*q_0(t)*q_3(t) + 2*A_X(t)*q_1(t)*q_2(t) + A_Y(t)*q_0(t)**2 - A_Y(t)*q_1(t)**2 + A_Y(t)*q_2(t)**2 - A_Y(t)*q_3(t)**2 - 2*A_Z(t)*q_0(t)*q_1(t) + 2*A_Z(t)*q_2(t)*q_3(t) + g_y(t)*q_0(t)**2 + g_y(t)*q_1(t)**2 + g_y(t)*q_2(t)**2 + g_y(t)*q_3(t)**2)/(q_0(t)**2 + q_1(t)**2 + q_2(t)**2 + q_3(t)**2))"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    },
    {
     "data": {
      "text/latex": [
       "$\\displaystyle \\frac{d}{d t} \\operatorname{v_{z}}{\\left(t \\right)} = \\frac{- 2 \\operatorname{A_{X}}{\\left(t \\right)} \\operatorname{q_{0}}{\\left(t \\right)} \\operatorname{q_{2}}{\\left(t \\right)} + 2 \\operatorname{A_{X}}{\\left(t \\right)} \\operatorname{q_{1}}{\\left(t \\right)} \\operatorname{q_{3}}{\\left(t \\right)} + 2 \\operatorname{A_{Y}}{\\left(t \\right)} \\operatorname{q_{0}}{\\left(t \\right)} \\operatorname{q_{1}}{\\left(t \\right)} + 2 \\operatorname{A_{Y}}{\\left(t \\right)} \\operatorname{q_{2}}{\\left(t \\right)} \\operatorname{q_{3}}{\\left(t \\right)} + \\operatorname{A_{Z}}{\\left(t \\right)} \\operatorname{q_{0}}^{2}{\\left(t \\right)} - \\operatorname{A_{Z}}{\\left(t \\right)} \\operatorname{q_{1}}^{2}{\\left(t \\right)} - \\operatorname{A_{Z}}{\\left(t \\right)} \\operatorname{q_{2}}^{2}{\\left(t \\right)} + \\operatorname{A_{Z}}{\\left(t \\right)} \\operatorname{q_{3}}^{2}{\\left(t \\right)} + \\operatorname{g_{z}}{\\left(t \\right)} \\operatorname{q_{0}}^{2}{\\left(t \\right)} + \\operatorname{g_{z}}{\\left(t \\right)} \\operatorname{q_{1}}^{2}{\\left(t \\right)} + \\operatorname{g_{z}}{\\left(t \\right)} \\operatorname{q_{2}}^{2}{\\left(t \\right)} + \\operatorname{g_{z}}{\\left(t \\right)} \\operatorname{q_{3}}^{2}{\\left(t \\right)}}{\\operatorname{q_{0}}^{2}{\\left(t \\right)} + \\operatorname{q_{1}}^{2}{\\left(t \\right)} + \\operatorname{q_{2}}^{2}{\\left(t \\right)} + \\operatorname{q_{3}}^{2}{\\left(t \\right)}}$"
      ],
      "text/plain": [
       "Eq(Derivative(v_z(t), t), (-2*A_X(t)*q_0(t)*q_2(t) + 2*A_X(t)*q_1(t)*q_3(t) + 2*A_Y(t)*q_0(t)*q_1(t) + 2*A_Y(t)*q_2(t)*q_3(t) + A_Z(t)*q_0(t)**2 - A_Z(t)*q_1(t)**2 - A_Z(t)*q_2(t)**2 + A_Z(t)*q_3(t)**2 + g_z(t)*q_0(t)**2 + g_z(t)*q_1(t)**2 + g_z(t)*q_2(t)**2 + g_z(t)*q_3(t)**2)/(q_0(t)**2 + q_1(t)**2 + q_2(t)**2 + q_3(t)**2))"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    }
   ],
   "source": [
    "translational_acceleration_in_I_vec = translation_acceleration_due_to_gravity + translation_acceleration_due_to_body\n",
    "translational_acceleration_in_I_expr = translational_acceleration_in_I_vec.to_matrix(I).simplify()\n",
    "inertial_vel_I_deriv_dict = sp.solve(translational_acceleration_in_I_expr - sp.Matrix(inertial_vel_I.diff(t)), inertial_vel_I.diff(t))\n",
    "\n",
    "display_dict(inertial_vel_I_deriv_dict)\n",
    "# TODO: simplify the gravity components to reduce number of multiply and add"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Inertial angular velocity derivative\n",
    "\n",
    "The derivative of the angular velocity of the vehicle body relative to the inertial frame expressed in body-fixed coordinates is a simple time-derivative in the inertial frame."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 8,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/latex": [
       "$\\displaystyle \\frac{d}{d t} \\omega_{X}{\\left(t \\right)} = \\alpha_{X}{\\left(t \\right)}$"
      ],
      "text/plain": [
       "Eq(Derivative(omega_X(t), t), alpha_X(t))"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    },
    {
     "data": {
      "text/latex": [
       "$\\displaystyle \\frac{d}{d t} \\omega_{Y}{\\left(t \\right)} = \\alpha_{Y}{\\left(t \\right)}$"
      ],
      "text/plain": [
       "Eq(Derivative(omega_Y(t), t), alpha_Y(t))"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    },
    {
     "data": {
      "text/latex": [
       "$\\displaystyle \\frac{d}{d t} \\omega_{Z}{\\left(t \\right)} = \\alpha_{Z}{\\left(t \\right)}$"
      ],
      "text/plain": [
       "Eq(Derivative(omega_Z(t), t), alpha_Z(t))"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    }
   ],
   "source": [
    "I_ang_vel_B_deriv_dict = sp.solve(I_ang_vel_B.diff(t) - I_ang_acc_B, I_ang_vel_B.diff(t))\n",
    "display_dict(I_ang_vel_B_deriv_dict)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Combine and prepare for codegen\n",
    "\n",
    "We combine the derivative dictionaries into the `kinematics_state_equation_dict` to provide expressions for the state derivative function to return (the \"expression dictionary\" for the function to be implemented by codegen). We leverage the fact that Python 3.6+ has ordered dictionaries to the \"extra-assignments\" dictionary `kinematics_state_extra_dict` for assigning values prior to CSE."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 9,
   "metadata": {},
   "outputs": [],
   "source": [
    "kinematics_state_equation_dict = {}\n",
    "for deriv_dict in inertial_pos_I_deriv_dict, inertial_vel_I_deriv_dict, q_deriv_dict, I_ang_vel_B_deriv_dict:\n",
    "    kinematics_state_equation_dict.update(deriv_dict)\n",
    "\n",
    "kinematics_state_extra_dict = {}\n",
    "# If using nutation/precession, would need to compute planet fixed rectangular position before calling gravity\n",
    "kinematics_state_extra_dict[g] = gravity_func\n",
    "    "
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# Kinematic Outputs\n",
    "\n",
    "Next we construct the additional outputs that are used in the `Vehicle` models for common flight vehicles. The `kinematics_output_dict` is expression dictionary and `kinematics_output_equation_extra_assinments_dict` is the extra-assignments dictionary.\n",
    "\n",
    "## Initialize with planetodetics, gravity, and winds"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 10,
   "metadata": {},
   "outputs": [],
   "source": [
    "kinematics_output_dict = {}\n",
    "kinematics_output_equation_extra_assinments_dict = {}\n",
    "\n",
    "# output all of the kinematic state veriables\n",
    "for state_var in kinematics_state:\n",
    "    kinematics_output_dict[state_var] = state_var\n",
    "\n",
    "if INCLUDE_NUTATION_PRECESSION:\n",
    "    # compute the rotation matrix and planet-fixed position in rectangular coordinates\n",
    "    kinematics_output_equation_extra_assinments_dict[pci2pcf_rotation_matrix] = pci2pcf_rotation_matrix_function\n",
    "    kinematics_output_equation_extra_assinments_dict[planet_fixed_position] = pci2pcf_rotation_matrix_function * inertial_pos_I\n",
    "    \n",
    "# use the planetodesy model to compute longitude, latitude, and altitude\n",
    "kinematics_output_equation_extra_assinments_dict[sp.Array((lamda_D, phi_D, h_D))] = pcf2pd\n",
    "\n",
    "\n",
    "if not INCLUDE_NUTATION_PRECESSION:\n",
    "    # without nutation and precession, the side-reel time is accounted for by shifting the latitude\n",
    "    kinematics_output_equation_extra_assinments_dict[lamda_D] = lamda_D - omega_p*t\n",
    "\n",
    "kinematics_output_dict[lamda_D] = lamda_D\n",
    "kinematics_output_dict[phi_D] = phi_D\n",
    "kinematics_output_dict[h_D] = h_D\n",
    "\n",
    "# wind model assumed to depend on the planetodetic position\n",
    "kinematics_output_equation_extra_assinments_dict[W_NED] = winds_func\n",
    "\n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Calculate local-to-body euler angles\n",
    "\n",
    "Here we compute the euler angles roll ($\\phi$), pitch ($\\theta$), and yaw ($\\phi$). We construct the local NED to body rotation matrix, then determine the euler angles following \"Review of Attitude Representations Used for Aircraft Kinematics\" by Phillips et. al., 2001."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 11,
   "metadata": {},
   "outputs": [],
   "source": [
    "NED_rotation_B = NED.dcm(I) @ I.dcm(B) # equivalent to NED.dcm(B), \n",
    "NED_rotation_B_simplified = NED_rotation_B.simplify()\n",
    "\n",
    "# Phillips eq. 1 and 25 defines Rotatation matrix from inertia to body\n",
    "# NED_rotation_B is body to NED\n",
    "# this follows Eq. 53 from Phillips.\n",
    "\n",
    "kinematics_output_dict[psi] = sp.atan2(NED_rotation_B_simplified[1,0], NED_rotation_B_simplified[0,0])\n",
    "kinematics_output_dict[theta] = sp.asin(-NED_rotation_B_simplified[2,0]) \n",
    "kinematics_output_dict[phi] = sp.atan2(NED_rotation_B_simplified[2,1], NED_rotation_B_simplified[2,2])\n",
    "\n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Atmosphere models"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 12,
   "metadata": {},
   "outputs": [],
   "source": [
    "kinematics_output_equation_extra_assinments_dict[sp.Array([density, speed_of_sound, viscosity,])] = atmosphere_func\n",
    "\n",
    "kinematics_output_dict[density] = density\n",
    "kinematics_output_dict[speed_of_sound] = speed_of_sound\n",
    "kinematics_output_dict[viscosity] = viscosity"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Computing elements for aerodynamics\n",
    "\n",
    "\n",
    "Here, we calculate the wind experienced on the body by determining the velocity of the of the vehicle's center of mass in the planet-fixed frame $P$, incorporating the winds, and expressing the computed wind in body-fixed coordinates."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 13,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/latex": [
       "$\\displaystyle \\left[\\begin{matrix}\\frac{2 \\left(- \\operatorname{q_{0}}{\\left(t \\right)} \\operatorname{q_{2}}{\\left(t \\right)} + \\operatorname{q_{1}}{\\left(t \\right)} \\operatorname{q_{3}}{\\left(t \\right)}\\right) \\left(\\operatorname{W_{D}}{\\left(t \\right)} \\sin{\\left(\\phi_{D}{\\left(t \\right)} \\right)} - \\operatorname{W_{N}}{\\left(t \\right)} \\cos{\\left(\\phi_{D}{\\left(t \\right)} \\right)} + \\operatorname{v_{z}}{\\left(t \\right)}\\right) + 2 \\left(\\operatorname{q_{0}}{\\left(t \\right)} \\operatorname{q_{3}}{\\left(t \\right)} + \\operatorname{q_{1}}{\\left(t \\right)} \\operatorname{q_{2}}{\\left(t \\right)}\\right) \\left(- \\omega_{p} \\operatorname{p_{x}}{\\left(t \\right)} + \\operatorname{W_{D}}{\\left(t \\right)} \\sin{\\left(\\omega_{p} t + \\lambda_{D}{\\left(t \\right)} \\right)} \\cos{\\left(\\phi_{D}{\\left(t \\right)} \\right)} - \\operatorname{W_{E}}{\\left(t \\right)} \\cos{\\left(\\omega_{p} t + \\lambda_{D}{\\left(t \\right)} \\right)} + \\operatorname{W_{N}}{\\left(t \\right)} \\sin{\\left(\\omega_{p} t + \\lambda_{D}{\\left(t \\right)} \\right)} \\sin{\\left(\\phi_{D}{\\left(t \\right)} \\right)} + \\operatorname{v_{y}}{\\left(t \\right)}\\right) + \\left(\\operatorname{q_{0}}^{2}{\\left(t \\right)} + \\operatorname{q_{1}}^{2}{\\left(t \\right)} - \\operatorname{q_{2}}^{2}{\\left(t \\right)} - \\operatorname{q_{3}}^{2}{\\left(t \\right)}\\right) \\left(\\omega_{p} \\operatorname{p_{y}}{\\left(t \\right)} + \\operatorname{W_{D}}{\\left(t \\right)} \\cos{\\left(\\omega_{p} t + \\lambda_{D}{\\left(t \\right)} \\right)} \\cos{\\left(\\phi_{D}{\\left(t \\right)} \\right)} + \\operatorname{W_{E}}{\\left(t \\right)} \\sin{\\left(\\omega_{p} t + \\lambda_{D}{\\left(t \\right)} \\right)} + \\operatorname{W_{N}}{\\left(t \\right)} \\sin{\\left(\\phi_{D}{\\left(t \\right)} \\right)} \\cos{\\left(\\omega_{p} t + \\lambda_{D}{\\left(t \\right)} \\right)} + \\operatorname{v_{x}}{\\left(t \\right)}\\right)}{\\operatorname{q_{0}}^{2}{\\left(t \\right)} + \\operatorname{q_{1}}^{2}{\\left(t \\right)} + \\operatorname{q_{2}}^{2}{\\left(t \\right)} + \\operatorname{q_{3}}^{2}{\\left(t \\right)}}\\\\\\frac{2 \\left(\\operatorname{q_{0}}{\\left(t \\right)} \\operatorname{q_{1}}{\\left(t \\right)} + \\operatorname{q_{2}}{\\left(t \\right)} \\operatorname{q_{3}}{\\left(t \\right)}\\right) \\left(\\operatorname{W_{D}}{\\left(t \\right)} \\sin{\\left(\\phi_{D}{\\left(t \\right)} \\right)} - \\operatorname{W_{N}}{\\left(t \\right)} \\cos{\\left(\\phi_{D}{\\left(t \\right)} \\right)} + \\operatorname{v_{z}}{\\left(t \\right)}\\right) + 2 \\left(- \\operatorname{q_{0}}{\\left(t \\right)} \\operatorname{q_{3}}{\\left(t \\right)} + \\operatorname{q_{1}}{\\left(t \\right)} \\operatorname{q_{2}}{\\left(t \\right)}\\right) \\left(\\omega_{p} \\operatorname{p_{y}}{\\left(t \\right)} + \\operatorname{W_{D}}{\\left(t \\right)} \\cos{\\left(\\omega_{p} t + \\lambda_{D}{\\left(t \\right)} \\right)} \\cos{\\left(\\phi_{D}{\\left(t \\right)} \\right)} + \\operatorname{W_{E}}{\\left(t \\right)} \\sin{\\left(\\omega_{p} t + \\lambda_{D}{\\left(t \\right)} \\right)} + \\operatorname{W_{N}}{\\left(t \\right)} \\sin{\\left(\\phi_{D}{\\left(t \\right)} \\right)} \\cos{\\left(\\omega_{p} t + \\lambda_{D}{\\left(t \\right)} \\right)} + \\operatorname{v_{x}}{\\left(t \\right)}\\right) + \\left(\\operatorname{q_{0}}^{2}{\\left(t \\right)} - \\operatorname{q_{1}}^{2}{\\left(t \\right)} + \\operatorname{q_{2}}^{2}{\\left(t \\right)} - \\operatorname{q_{3}}^{2}{\\left(t \\right)}\\right) \\left(- \\omega_{p} \\operatorname{p_{x}}{\\left(t \\right)} + \\operatorname{W_{D}}{\\left(t \\right)} \\sin{\\left(\\omega_{p} t + \\lambda_{D}{\\left(t \\right)} \\right)} \\cos{\\left(\\phi_{D}{\\left(t \\right)} \\right)} - \\operatorname{W_{E}}{\\left(t \\right)} \\cos{\\left(\\omega_{p} t + \\lambda_{D}{\\left(t \\right)} \\right)} + \\operatorname{W_{N}}{\\left(t \\right)} \\sin{\\left(\\omega_{p} t + \\lambda_{D}{\\left(t \\right)} \\right)} \\sin{\\left(\\phi_{D}{\\left(t \\right)} \\right)} + \\operatorname{v_{y}}{\\left(t \\right)}\\right)}{\\operatorname{q_{0}}^{2}{\\left(t \\right)} + \\operatorname{q_{1}}^{2}{\\left(t \\right)} + \\operatorname{q_{2}}^{2}{\\left(t \\right)} + \\operatorname{q_{3}}^{2}{\\left(t \\right)}}\\\\\\frac{2 \\left(- \\operatorname{q_{0}}{\\left(t \\right)} \\operatorname{q_{1}}{\\left(t \\right)} + \\operatorname{q_{2}}{\\left(t \\right)} \\operatorname{q_{3}}{\\left(t \\right)}\\right) \\left(- \\omega_{p} \\operatorname{p_{x}}{\\left(t \\right)} + \\operatorname{W_{D}}{\\left(t \\right)} \\sin{\\left(\\omega_{p} t + \\lambda_{D}{\\left(t \\right)} \\right)} \\cos{\\left(\\phi_{D}{\\left(t \\right)} \\right)} - \\operatorname{W_{E}}{\\left(t \\right)} \\cos{\\left(\\omega_{p} t + \\lambda_{D}{\\left(t \\right)} \\right)} + \\operatorname{W_{N}}{\\left(t \\right)} \\sin{\\left(\\omega_{p} t + \\lambda_{D}{\\left(t \\right)} \\right)} \\sin{\\left(\\phi_{D}{\\left(t \\right)} \\right)} + \\operatorname{v_{y}}{\\left(t \\right)}\\right) + 2 \\left(\\operatorname{q_{0}}{\\left(t \\right)} \\operatorname{q_{2}}{\\left(t \\right)} + \\operatorname{q_{1}}{\\left(t \\right)} \\operatorname{q_{3}}{\\left(t \\right)}\\right) \\left(\\omega_{p} \\operatorname{p_{y}}{\\left(t \\right)} + \\operatorname{W_{D}}{\\left(t \\right)} \\cos{\\left(\\omega_{p} t + \\lambda_{D}{\\left(t \\right)} \\right)} \\cos{\\left(\\phi_{D}{\\left(t \\right)} \\right)} + \\operatorname{W_{E}}{\\left(t \\right)} \\sin{\\left(\\omega_{p} t + \\lambda_{D}{\\left(t \\right)} \\right)} + \\operatorname{W_{N}}{\\left(t \\right)} \\sin{\\left(\\phi_{D}{\\left(t \\right)} \\right)} \\cos{\\left(\\omega_{p} t + \\lambda_{D}{\\left(t \\right)} \\right)} + \\operatorname{v_{x}}{\\left(t \\right)}\\right) + \\left(\\operatorname{W_{D}}{\\left(t \\right)} \\sin{\\left(\\phi_{D}{\\left(t \\right)} \\right)} - \\operatorname{W_{N}}{\\left(t \\right)} \\cos{\\left(\\phi_{D}{\\left(t \\right)} \\right)} + \\operatorname{v_{z}}{\\left(t \\right)}\\right) \\left(\\operatorname{q_{0}}^{2}{\\left(t \\right)} - \\operatorname{q_{1}}^{2}{\\left(t \\right)} - \\operatorname{q_{2}}^{2}{\\left(t \\right)} + \\operatorname{q_{3}}^{2}{\\left(t \\right)}\\right)}{\\operatorname{q_{0}}^{2}{\\left(t \\right)} + \\operatorname{q_{1}}^{2}{\\left(t \\right)} + \\operatorname{q_{2}}^{2}{\\left(t \\right)} + \\operatorname{q_{3}}^{2}{\\left(t \\right)}}\\end{matrix}\\right]$"
      ],
      "text/plain": [
       "Matrix([\n",
       "[(2*(-q_0(t)*q_2(t) + q_1(t)*q_3(t))*(W_D(t)*sin(phi_D(t)) - W_N(t)*cos(phi_D(t)) + v_z(t)) + 2*(q_0(t)*q_3(t) + q_1(t)*q_2(t))*(-omega_p*p_x(t) + W_D(t)*sin(omega_p*t + lamda_D(t))*cos(phi_D(t)) - W_E(t)*cos(omega_p*t + lamda_D(t)) + W_N(t)*sin(omega_p*t + lamda_D(t))*sin(phi_D(t)) + v_y(t)) + (q_0(t)**2 + q_1(t)**2 - q_2(t)**2 - q_3(t)**2)*(omega_p*p_y(t) + W_D(t)*cos(omega_p*t + lamda_D(t))*cos(phi_D(t)) + W_E(t)*sin(omega_p*t + lamda_D(t)) + W_N(t)*sin(phi_D(t))*cos(omega_p*t + lamda_D(t)) + v_x(t)))/(q_0(t)**2 + q_1(t)**2 + q_2(t)**2 + q_3(t)**2)],\n",
       "[(2*(q_0(t)*q_1(t) + q_2(t)*q_3(t))*(W_D(t)*sin(phi_D(t)) - W_N(t)*cos(phi_D(t)) + v_z(t)) + 2*(-q_0(t)*q_3(t) + q_1(t)*q_2(t))*(omega_p*p_y(t) + W_D(t)*cos(omega_p*t + lamda_D(t))*cos(phi_D(t)) + W_E(t)*sin(omega_p*t + lamda_D(t)) + W_N(t)*sin(phi_D(t))*cos(omega_p*t + lamda_D(t)) + v_x(t)) + (q_0(t)**2 - q_1(t)**2 + q_2(t)**2 - q_3(t)**2)*(-omega_p*p_x(t) + W_D(t)*sin(omega_p*t + lamda_D(t))*cos(phi_D(t)) - W_E(t)*cos(omega_p*t + lamda_D(t)) + W_N(t)*sin(omega_p*t + lamda_D(t))*sin(phi_D(t)) + v_y(t)))/(q_0(t)**2 + q_1(t)**2 + q_2(t)**2 + q_3(t)**2)],\n",
       "[(2*(-q_0(t)*q_1(t) + q_2(t)*q_3(t))*(-omega_p*p_x(t) + W_D(t)*sin(omega_p*t + lamda_D(t))*cos(phi_D(t)) - W_E(t)*cos(omega_p*t + lamda_D(t)) + W_N(t)*sin(omega_p*t + lamda_D(t))*sin(phi_D(t)) + v_y(t)) + 2*(q_0(t)*q_2(t) + q_1(t)*q_3(t))*(omega_p*p_y(t) + W_D(t)*cos(omega_p*t + lamda_D(t))*cos(phi_D(t)) + W_E(t)*sin(omega_p*t + lamda_D(t)) + W_N(t)*sin(phi_D(t))*cos(omega_p*t + lamda_D(t)) + v_x(t)) + (W_D(t)*sin(phi_D(t)) - W_N(t)*cos(phi_D(t)) + v_z(t))*(q_0(t)**2 - q_1(t)**2 - q_2(t)**2 + q_3(t)**2))/(q_0(t)**2 + q_1(t)**2 + q_2(t)**2 + q_3(t)**2)]])"
      ]
     },
     "execution_count": 13,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "B_cm.v1pt_theory(I0, P, I) # calculate the velocity of the vehicle's center of mass in the planet-fixed frame\n",
    "local_wind = W_N*NED.x + W_E*NED.y + W_D*NED.z\n",
    "body_wind = B_cm.vel(P) - local_wind # incorporate the wind model\n",
    "body_wind_I = body_wind.to_matrix(I).simplify() # express in inertial coordinates\n",
    "\n",
    "body_wind_B = B.dcm(I).simplify()@body_wind_I # express the winds in body-fixed coordinates\n",
    "body_wind_B_simplified = body_wind_B.simplify() # simplify\n",
    "\n",
    "body_wind_B_simplified"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Next we compute the total true wind $V_T$ (the magnitude of the wind calculated above) and the angles of attack and sideslip."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 14,
   "metadata": {},
   "outputs": [],
   "source": [
    "V_T_simp = (body_wind.magnitude()**2).simplify()\n",
    "kinematics_output_dict[V_T] = sp.sqrt(V_T_simp*sp.Heaviside(V_T_simp))\n",
    "kinematics_output_dict[alpha] = sp.atan2(body_wind_B_simplified[2], body_wind_B_simplified[0])\n",
    "kinematics_output_dict[beta] = sp.asin(\n",
    "    sp.Piecewise( # This piecewise expression is used to provide the correct output at singularities\n",
    "        (1., (body_wind_B_simplified[1]/kinematics_output_dict[V_T])>1.0), \n",
    "        (-1., (body_wind_B_simplified[1]/kinematics_output_dict[V_T])<-1.0),\n",
    "        (body_wind_B_simplified[1]/kinematics_output_dict[V_T], kinematics_output_dict[V_T]>0.),\n",
    "        (0., True)\n",
    "    )\n",
    ")"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Translational velocity in local NED frame\n",
    "\n",
    "Here, we calculate the local velocity (in the planet centered planet fixed frame $P$)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 15,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/latex": [
       "$\\displaystyle \\operatorname{V_{N}}{\\left(t \\right)} = \\omega_{p} \\operatorname{p_{x}}{\\left(t \\right)} \\sin{\\left(\\omega_{p} t + \\lambda_{D}{\\left(t \\right)} \\right)} \\sin{\\left(\\phi_{D}{\\left(t \\right)} \\right)} - \\omega_{p} \\operatorname{p_{y}}{\\left(t \\right)} \\sin{\\left(\\phi_{D}{\\left(t \\right)} \\right)} \\cos{\\left(\\omega_{p} t + \\lambda_{D}{\\left(t \\right)} \\right)} - \\operatorname{v_{x}}{\\left(t \\right)} \\sin{\\left(\\phi_{D}{\\left(t \\right)} \\right)} \\cos{\\left(\\omega_{p} t + \\lambda_{D}{\\left(t \\right)} \\right)} - \\operatorname{v_{y}}{\\left(t \\right)} \\sin{\\left(\\omega_{p} t + \\lambda_{D}{\\left(t \\right)} \\right)} \\sin{\\left(\\phi_{D}{\\left(t \\right)} \\right)} + \\operatorname{v_{z}}{\\left(t \\right)} \\cos{\\left(\\phi_{D}{\\left(t \\right)} \\right)}$"
      ],
      "text/plain": [
       "Eq(V_N(t), omega_p*p_x(t)*sin(omega_p*t + lamda_D(t))*sin(phi_D(t)) - omega_p*p_y(t)*sin(phi_D(t))*cos(omega_p*t + lamda_D(t)) - v_x(t)*sin(phi_D(t))*cos(omega_p*t + lamda_D(t)) - v_y(t)*sin(omega_p*t + lamda_D(t))*sin(phi_D(t)) + v_z(t)*cos(phi_D(t)))"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    },
    {
     "data": {
      "text/latex": [
       "$\\displaystyle \\operatorname{V_{E}}{\\left(t \\right)} = - \\omega_{p} \\operatorname{p_{x}}{\\left(t \\right)} \\cos{\\left(\\omega_{p} t + \\lambda_{D}{\\left(t \\right)} \\right)} - \\omega_{p} \\operatorname{p_{y}}{\\left(t \\right)} \\sin{\\left(\\omega_{p} t + \\lambda_{D}{\\left(t \\right)} \\right)} - \\operatorname{v_{x}}{\\left(t \\right)} \\sin{\\left(\\omega_{p} t + \\lambda_{D}{\\left(t \\right)} \\right)} + \\operatorname{v_{y}}{\\left(t \\right)} \\cos{\\left(\\omega_{p} t + \\lambda_{D}{\\left(t \\right)} \\right)}$"
      ],
      "text/plain": [
       "Eq(V_E(t), -omega_p*p_x(t)*cos(omega_p*t + lamda_D(t)) - omega_p*p_y(t)*sin(omega_p*t + lamda_D(t)) - v_x(t)*sin(omega_p*t + lamda_D(t)) + v_y(t)*cos(omega_p*t + lamda_D(t)))"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    },
    {
     "data": {
      "text/latex": [
       "$\\displaystyle \\operatorname{V_{D}}{\\left(t \\right)} = \\omega_{p} \\operatorname{p_{x}}{\\left(t \\right)} \\sin{\\left(\\omega_{p} t + \\lambda_{D}{\\left(t \\right)} \\right)} \\cos{\\left(\\phi_{D}{\\left(t \\right)} \\right)} - \\omega_{p} \\operatorname{p_{y}}{\\left(t \\right)} \\cos{\\left(\\omega_{p} t + \\lambda_{D}{\\left(t \\right)} \\right)} \\cos{\\left(\\phi_{D}{\\left(t \\right)} \\right)} - \\operatorname{v_{x}}{\\left(t \\right)} \\cos{\\left(\\omega_{p} t + \\lambda_{D}{\\left(t \\right)} \\right)} \\cos{\\left(\\phi_{D}{\\left(t \\right)} \\right)} - \\operatorname{v_{y}}{\\left(t \\right)} \\sin{\\left(\\omega_{p} t + \\lambda_{D}{\\left(t \\right)} \\right)} \\cos{\\left(\\phi_{D}{\\left(t \\right)} \\right)} - \\operatorname{v_{z}}{\\left(t \\right)} \\sin{\\left(\\phi_{D}{\\left(t \\right)} \\right)}$"
      ],
      "text/plain": [
       "Eq(V_D(t), omega_p*p_x(t)*sin(omega_p*t + lamda_D(t))*cos(phi_D(t)) - omega_p*p_y(t)*cos(omega_p*t + lamda_D(t))*cos(phi_D(t)) - v_x(t)*cos(omega_p*t + lamda_D(t))*cos(phi_D(t)) - v_y(t)*sin(omega_p*t + lamda_D(t))*cos(phi_D(t)) - v_z(t)*sin(phi_D(t)))"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    }
   ],
   "source": [
    "B_cm.v1pt_theory(I0, P, I)\n",
    "V_NED_expr = B_cm.vel(P).to_matrix(NED)\n",
    "V_NED_expr_dict = sp.solve(V_NED_expr - sp.Matrix(V_NED), V_NED)\n",
    "\n",
    "V_NED_cse = sp.cse(V_NED_expr, symbols=sp.numbered_symbols(prefix='V_NED_'), order='none')\n",
    "V_NED_cse_assignments = {k: v for k,v in V_NED_cse[0]}\n",
    "V_NED_cse_assignments.update({k: v for k, v in zip(V_NED, V_NED_cse[1][0])})\n",
    "kinematics_output_equation_extra_assinments_dict.update(V_NED_cse_assignments)\n",
    "\n",
    "display_dict(V_NED_expr_dict)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Angular velocity in local NED frame\n",
    "\n",
    "Here we calculate the angular velocity components of the vehicle in air, used for the dynamic (or damping) aerodynamic moment derivatives."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 16,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/latex": [
       "$\\displaystyle \\dot{\\phi}_{D}{\\left(t \\right)} = \\frac{\\operatorname{V_{N}}{\\left(t \\right)}}{\\frac{a \\left(1 - f\\right)^{2}}{\\left(- \\left(1 - \\left(1 - f\\right)^{2}\\right) \\sin^{2}{\\left(\\phi_{D}{\\left(t \\right)} \\right)} + 1\\right)^{\\frac{3}{2}}} + \\operatorname{h_{D}}{\\left(t \\right)}}$"
      ],
      "text/plain": [
       "Eq(phidot_D(t), V_N(t)/(a*(1 - f)**2/(-(1 - (1 - f)**2)*sin(phi_D(t))**2 + 1)**(3/2) + h_D(t)))"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    },
    {
     "data": {
      "text/latex": [
       "$\\displaystyle \\dot{\\lambda}_{D}{\\left(t \\right)} = \\frac{\\operatorname{V_{E}}{\\left(t \\right)}}{\\left(\\frac{a}{\\sqrt{- \\left(1 - \\left(1 - f\\right)^{2}\\right) \\sin^{2}{\\left(\\phi_{D}{\\left(t \\right)} \\right)} + 1}} + \\operatorname{h_{D}}{\\left(t \\right)}\\right) \\cos{\\left(\\phi_{D}{\\left(t \\right)} \\right)}}$"
      ],
      "text/plain": [
       "Eq(lamdadot_D(t), V_E(t)/((a/sqrt(-(1 - (1 - f)**2)*sin(phi_D(t))**2 + 1) + h_D(t))*cos(phi_D(t))))"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    },
    {
     "data": {
      "text/latex": [
       "$\\displaystyle \\dot{h}_{D}{\\left(t \\right)} = - \\operatorname{V_{D}}{\\left(t \\right)}$"
      ],
      "text/plain": [
       "Eq(hdot_D(t), -V_D(t))"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    }
   ],
   "source": [
    "e_squared = 1-(1-f)**2 # eccentricity squared\n",
    "# radius in prime meridian\n",
    "rho_M_expr = a*(1-e_squared)/sp.sqrt(1-e_squared*sp.sin(phi_D)**2)**(3)\n",
    "# radius in prime vertical\n",
    "rho_V_expr = a/sp.sqrt(1-e_squared*sp.sin(phi_D)**2)\n",
    "\n",
    "navigation_kinematics = {\n",
    "    phi_D_dot: V_N / (rho_M_expr + h_D),\n",
    "    lamda_D_dot: V_E/((rho_V_expr + h_D)*sp.cos(phi_D)),\n",
    "    h_D_dot: -V_D\n",
    "}\n",
    "\n",
    "display_dict(navigation_kinematics)\n",
    "\n",
    "navigation_cse = sp.cse(sp.Matrix([navigation_kinematics[key] for key in navigation_kinematics]), symbols=sp.numbered_symbols(prefix='navigation_'), order='none')\n",
    "\n",
    "navigation_cse_assignments = {k: v for k,v in navigation_cse[0]}\n",
    "navigation_cse_assignments.update({k: v for k, v in zip(navigation_kinematics.keys(), navigation_cse[1][0])})\n",
    "kinematics_output_equation_extra_assinments_dict.update(navigation_cse_assignments)\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 17,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/latex": [
       "$\\displaystyle \\operatorname{p_{B}}{\\left(t \\right)} = \\frac{2 \\omega_{p} \\operatorname{q_{0}}{\\left(t \\right)} \\operatorname{q_{2}}{\\left(t \\right)} - 2 \\omega_{p} \\operatorname{q_{1}}{\\left(t \\right)} \\operatorname{q_{3}}{\\left(t \\right)} + 2 \\dot{\\lambda}_{D}{\\left(t \\right)} \\operatorname{q_{0}}{\\left(t \\right)} \\operatorname{q_{2}}{\\left(t \\right)} - 2 \\dot{\\lambda}_{D}{\\left(t \\right)} \\operatorname{q_{1}}{\\left(t \\right)} \\operatorname{q_{3}}{\\left(t \\right)} + \\omega_{X}{\\left(t \\right)} \\operatorname{q_{0}}^{2}{\\left(t \\right)} + \\omega_{X}{\\left(t \\right)} \\operatorname{q_{1}}^{2}{\\left(t \\right)} + \\omega_{X}{\\left(t \\right)} \\operatorname{q_{2}}^{2}{\\left(t \\right)} + \\omega_{X}{\\left(t \\right)} \\operatorname{q_{3}}^{2}{\\left(t \\right)} - \\dot{\\phi}_{D}{\\left(t \\right)} \\operatorname{q_{0}}^{2}{\\left(t \\right)} \\sin{\\left(\\omega_{p} t + \\lambda_{D}{\\left(t \\right)} \\right)} + 2 \\dot{\\phi}_{D}{\\left(t \\right)} \\operatorname{q_{0}}{\\left(t \\right)} \\operatorname{q_{3}}{\\left(t \\right)} \\cos{\\left(\\omega_{p} t + \\lambda_{D}{\\left(t \\right)} \\right)} - \\dot{\\phi}_{D}{\\left(t \\right)} \\operatorname{q_{1}}^{2}{\\left(t \\right)} \\sin{\\left(\\omega_{p} t + \\lambda_{D}{\\left(t \\right)} \\right)} + 2 \\dot{\\phi}_{D}{\\left(t \\right)} \\operatorname{q_{1}}{\\left(t \\right)} \\operatorname{q_{2}}{\\left(t \\right)} \\cos{\\left(\\omega_{p} t + \\lambda_{D}{\\left(t \\right)} \\right)} + \\dot{\\phi}_{D}{\\left(t \\right)} \\operatorname{q_{2}}^{2}{\\left(t \\right)} \\sin{\\left(\\omega_{p} t + \\lambda_{D}{\\left(t \\right)} \\right)} + \\dot{\\phi}_{D}{\\left(t \\right)} \\operatorname{q_{3}}^{2}{\\left(t \\right)} \\sin{\\left(\\omega_{p} t + \\lambda_{D}{\\left(t \\right)} \\right)}}{\\operatorname{q_{0}}^{2}{\\left(t \\right)} + \\operatorname{q_{1}}^{2}{\\left(t \\right)} + \\operatorname{q_{2}}^{2}{\\left(t \\right)} + \\operatorname{q_{3}}^{2}{\\left(t \\right)}}$"
      ],
      "text/plain": [
       "Eq(p_B(t), (2*omega_p*q_0(t)*q_2(t) - 2*omega_p*q_1(t)*q_3(t) + 2*lamdadot_D(t)*q_0(t)*q_2(t) - 2*lamdadot_D(t)*q_1(t)*q_3(t) + omega_X(t)*q_0(t)**2 + omega_X(t)*q_1(t)**2 + omega_X(t)*q_2(t)**2 + omega_X(t)*q_3(t)**2 - phidot_D(t)*q_0(t)**2*sin(omega_p*t + lamda_D(t)) + 2*phidot_D(t)*q_0(t)*q_3(t)*cos(omega_p*t + lamda_D(t)) - phidot_D(t)*q_1(t)**2*sin(omega_p*t + lamda_D(t)) + 2*phidot_D(t)*q_1(t)*q_2(t)*cos(omega_p*t + lamda_D(t)) + phidot_D(t)*q_2(t)**2*sin(omega_p*t + lamda_D(t)) + phidot_D(t)*q_3(t)**2*sin(omega_p*t + lamda_D(t)))/(q_0(t)**2 + q_1(t)**2 + q_2(t)**2 + q_3(t)**2))"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    },
    {
     "data": {
      "text/latex": [
       "$\\displaystyle \\operatorname{q_{B}}{\\left(t \\right)} = \\frac{- 2 \\omega_{p} \\operatorname{q_{0}}{\\left(t \\right)} \\operatorname{q_{1}}{\\left(t \\right)} - 2 \\omega_{p} \\operatorname{q_{2}}{\\left(t \\right)} \\operatorname{q_{3}}{\\left(t \\right)} - 2 \\dot{\\lambda}_{D}{\\left(t \\right)} \\operatorname{q_{0}}{\\left(t \\right)} \\operatorname{q_{1}}{\\left(t \\right)} - 2 \\dot{\\lambda}_{D}{\\left(t \\right)} \\operatorname{q_{2}}{\\left(t \\right)} \\operatorname{q_{3}}{\\left(t \\right)} + \\omega_{Y}{\\left(t \\right)} \\operatorname{q_{0}}^{2}{\\left(t \\right)} + \\omega_{Y}{\\left(t \\right)} \\operatorname{q_{1}}^{2}{\\left(t \\right)} + \\omega_{Y}{\\left(t \\right)} \\operatorname{q_{2}}^{2}{\\left(t \\right)} + \\omega_{Y}{\\left(t \\right)} \\operatorname{q_{3}}^{2}{\\left(t \\right)} + \\dot{\\phi}_{D}{\\left(t \\right)} \\operatorname{q_{0}}^{2}{\\left(t \\right)} \\cos{\\left(\\omega_{p} t + \\lambda_{D}{\\left(t \\right)} \\right)} + 2 \\dot{\\phi}_{D}{\\left(t \\right)} \\operatorname{q_{0}}{\\left(t \\right)} \\operatorname{q_{3}}{\\left(t \\right)} \\sin{\\left(\\omega_{p} t + \\lambda_{D}{\\left(t \\right)} \\right)} - \\dot{\\phi}_{D}{\\left(t \\right)} \\operatorname{q_{1}}^{2}{\\left(t \\right)} \\cos{\\left(\\omega_{p} t + \\lambda_{D}{\\left(t \\right)} \\right)} - 2 \\dot{\\phi}_{D}{\\left(t \\right)} \\operatorname{q_{1}}{\\left(t \\right)} \\operatorname{q_{2}}{\\left(t \\right)} \\sin{\\left(\\omega_{p} t + \\lambda_{D}{\\left(t \\right)} \\right)} + \\dot{\\phi}_{D}{\\left(t \\right)} \\operatorname{q_{2}}^{2}{\\left(t \\right)} \\cos{\\left(\\omega_{p} t + \\lambda_{D}{\\left(t \\right)} \\right)} - \\dot{\\phi}_{D}{\\left(t \\right)} \\operatorname{q_{3}}^{2}{\\left(t \\right)} \\cos{\\left(\\omega_{p} t + \\lambda_{D}{\\left(t \\right)} \\right)}}{\\operatorname{q_{0}}^{2}{\\left(t \\right)} + \\operatorname{q_{1}}^{2}{\\left(t \\right)} + \\operatorname{q_{2}}^{2}{\\left(t \\right)} + \\operatorname{q_{3}}^{2}{\\left(t \\right)}}$"
      ],
      "text/plain": [
       "Eq(q_B(t), (-2*omega_p*q_0(t)*q_1(t) - 2*omega_p*q_2(t)*q_3(t) - 2*lamdadot_D(t)*q_0(t)*q_1(t) - 2*lamdadot_D(t)*q_2(t)*q_3(t) + omega_Y(t)*q_0(t)**2 + omega_Y(t)*q_1(t)**2 + omega_Y(t)*q_2(t)**2 + omega_Y(t)*q_3(t)**2 + phidot_D(t)*q_0(t)**2*cos(omega_p*t + lamda_D(t)) + 2*phidot_D(t)*q_0(t)*q_3(t)*sin(omega_p*t + lamda_D(t)) - phidot_D(t)*q_1(t)**2*cos(omega_p*t + lamda_D(t)) - 2*phidot_D(t)*q_1(t)*q_2(t)*sin(omega_p*t + lamda_D(t)) + phidot_D(t)*q_2(t)**2*cos(omega_p*t + lamda_D(t)) - phidot_D(t)*q_3(t)**2*cos(omega_p*t + lamda_D(t)))/(q_0(t)**2 + q_1(t)**2 + q_2(t)**2 + q_3(t)**2))"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    },
    {
     "data": {
      "text/latex": [
       "$\\displaystyle \\operatorname{r_{B}}{\\left(t \\right)} = \\frac{- \\omega_{p} \\operatorname{q_{0}}^{2}{\\left(t \\right)} + \\omega_{p} \\operatorname{q_{1}}^{2}{\\left(t \\right)} + \\omega_{p} \\operatorname{q_{2}}^{2}{\\left(t \\right)} - \\omega_{p} \\operatorname{q_{3}}^{2}{\\left(t \\right)} - \\dot{\\lambda}_{D}{\\left(t \\right)} \\operatorname{q_{0}}^{2}{\\left(t \\right)} + \\dot{\\lambda}_{D}{\\left(t \\right)} \\operatorname{q_{1}}^{2}{\\left(t \\right)} + \\dot{\\lambda}_{D}{\\left(t \\right)} \\operatorname{q_{2}}^{2}{\\left(t \\right)} - \\dot{\\lambda}_{D}{\\left(t \\right)} \\operatorname{q_{3}}^{2}{\\left(t \\right)} + \\omega_{Z}{\\left(t \\right)} \\operatorname{q_{0}}^{2}{\\left(t \\right)} + \\omega_{Z}{\\left(t \\right)} \\operatorname{q_{1}}^{2}{\\left(t \\right)} + \\omega_{Z}{\\left(t \\right)} \\operatorname{q_{2}}^{2}{\\left(t \\right)} + \\omega_{Z}{\\left(t \\right)} \\operatorname{q_{3}}^{2}{\\left(t \\right)} - 2 \\dot{\\phi}_{D}{\\left(t \\right)} \\operatorname{q_{0}}{\\left(t \\right)} \\operatorname{q_{1}}{\\left(t \\right)} \\cos{\\left(\\omega_{p} t + \\lambda_{D}{\\left(t \\right)} \\right)} - 2 \\dot{\\phi}_{D}{\\left(t \\right)} \\operatorname{q_{0}}{\\left(t \\right)} \\operatorname{q_{2}}{\\left(t \\right)} \\sin{\\left(\\omega_{p} t + \\lambda_{D}{\\left(t \\right)} \\right)} - 2 \\dot{\\phi}_{D}{\\left(t \\right)} \\operatorname{q_{1}}{\\left(t \\right)} \\operatorname{q_{3}}{\\left(t \\right)} \\sin{\\left(\\omega_{p} t + \\lambda_{D}{\\left(t \\right)} \\right)} + 2 \\dot{\\phi}_{D}{\\left(t \\right)} \\operatorname{q_{2}}{\\left(t \\right)} \\operatorname{q_{3}}{\\left(t \\right)} \\cos{\\left(\\omega_{p} t + \\lambda_{D}{\\left(t \\right)} \\right)}}{\\operatorname{q_{0}}^{2}{\\left(t \\right)} + \\operatorname{q_{1}}^{2}{\\left(t \\right)} + \\operatorname{q_{2}}^{2}{\\left(t \\right)} + \\operatorname{q_{3}}^{2}{\\left(t \\right)}}$"
      ],
      "text/plain": [
       "Eq(r_B(t), (-omega_p*q_0(t)**2 + omega_p*q_1(t)**2 + omega_p*q_2(t)**2 - omega_p*q_3(t)**2 - lamdadot_D(t)*q_0(t)**2 + lamdadot_D(t)*q_1(t)**2 + lamdadot_D(t)*q_2(t)**2 - lamdadot_D(t)*q_3(t)**2 + omega_Z(t)*q_0(t)**2 + omega_Z(t)*q_1(t)**2 + omega_Z(t)*q_2(t)**2 + omega_Z(t)*q_3(t)**2 - 2*phidot_D(t)*q_0(t)*q_1(t)*cos(omega_p*t + lamda_D(t)) - 2*phidot_D(t)*q_0(t)*q_2(t)*sin(omega_p*t + lamda_D(t)) - 2*phidot_D(t)*q_1(t)*q_3(t)*sin(omega_p*t + lamda_D(t)) + 2*phidot_D(t)*q_2(t)*q_3(t)*cos(omega_p*t + lamda_D(t)))/(q_0(t)**2 + q_1(t)**2 + q_2(t)**2 + q_3(t)**2))"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    }
   ],
   "source": [
    "NED_ang_vel_B_expr_dict = sp.solve(B.ang_vel_in(NED).to_matrix(B) - sp.Matrix(NED_ang_vel_B), NED_ang_vel_B)\n",
    "NED_ang_vel_B_expr_simplified_dict = {}\n",
    "for k,v in NED_ang_vel_B_expr_dict.items():\n",
    "    NED_ang_vel_B_expr_simplified_dict[k] = v.subs(planetodetic_pos_derivs)\n",
    "kinematics_output_dict.update(NED_ang_vel_B_expr_simplified_dict)\n",
    "display_dict(NED_ang_vel_B_expr_simplified_dict)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Here we assign the local velocity and wind components in the local NED frame in the specified order."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 18,
   "metadata": {},
   "outputs": [],
   "source": [
    "for vel_var in V_NED:\n",
    "    kinematics_output_dict[vel_var] = vel_var\n",
    "\n",
    "# add the wind model outputs at the end to match the specified order\n",
    "for wind_var in W_NED:\n",
    "    kinematics_output_dict[wind_var] = wind_var"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# Computing inertial initial conditions\n",
    "\n",
    "Here we write a helper function that can compute the inertial initial conditions "
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 19,
   "metadata": {},
   "outputs": [],
   "source": [
    "B2 = NED.orientnew('B2', 'Body', [psi, theta, phi], '321')\n",
    "P_rotation_B = P.dcm(B2).simplify() # should be equivalent to LaRC slides [T^{BP}] at t=0, and taking into account PCI --> NED frames\n",
    "# LaRC provides a formula that seems to be the special case of Phillips 2001 for q0**2  > q1**2, q2**2, q3**2, with some sign flips\n",
    "# I'll stick with LaRC slides, but we should be aware of it based on\n",
    "\n",
    "\n",
    "\n",
    "half_root_trace_plus_1 = sp.sqrt(P_rotation_B.trace()+1)/2\n",
    "quaternion_from_spec = sp.Array([\n",
    "    half_root_trace_plus_1,\n",
    "    (P_rotation_B[2,1] - P_rotation_B[1,2])/(4*half_root_trace_plus_1),\n",
    "    (P_rotation_B[0,2] - P_rotation_B[2,0])/(4*half_root_trace_plus_1),\n",
    "    (P_rotation_B[1,0] - P_rotation_B[0,1])/(4*half_root_trace_plus_1),\n",
    "    \n",
    "])\n",
    "quaternion_from_euler_dict = sp.solve(quaternion_from_spec- q, q)\n",
    "\n",
    "inertial_trans_vel_from_relative_vel_NED_dict = sp.solve(B_cm.vel(P).to_matrix(NED) - sp.Matrix(V_NED), inertial_vel_I)\n",
    "\n",
    "inertial_ang_vel_from_local = sp.solve(B.ang_vel_in(NED).to_matrix(B).subs(planetodetic_pos_derivs) - sp.Matrix(NED_ang_vel_B), I_ang_vel_B)\n",
    "\n",
    "ic_from_pd_dict = {}\n",
    "ic_from_pd_dict.update(inertial_trans_vel_from_relative_vel_NED_dict)\n",
    "ic_from_pd_dict.update(quaternion_from_euler_dict)\n",
    "ic_from_pd_dict.update(inertial_ang_vel_from_local)\n",
    "ic_from_pd = kinematics_state.subs(ic_from_pd_dict).subs(omega_p*t, 0)\n",
    "\n",
    "sp.physics.mechanics.functions.find_dynamicsymbols(quaternion_from_spec)\n",
    "ic_extra = {}\n",
    "ic_extra[inertial_pos_I] = pd2pcf\n",
    "ic_extra.update(navigation_cse_assignments)\n",
    "ic_from_pd_args = (lamda_D, phi_D, h_D, V_N, V_E, V_D, psi, theta, phi, *NED_ang_vel_B)\n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# Compute local trim residual\n",
    "\n",
    "In order to trim the vehicle, we must calculate the acceleration of the vehicle's center of mass in terms of the local acceleration $\\dot{V}_N$, $\\dot{V}_E$, $\\dot{V}_D$. The vehicle is trim whent he local acceleration is zero."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 20,
   "metadata": {},
   "outputs": [],
   "source": [
    "V_NED_vec = np.sum(np.array(V_NED)*np.array([NED.x, NED.y, NED.z]))\n",
    "V_NED_dot_vec = np.sum(np.array(V_NED.diff(t))*np.array([NED.x, NED.y, NED.z]))\n",
    "\n",
    "B_cm.set_vel(P, V_NED_vec)\n",
    "V_NED_dot_residual = sp.Array(\n",
    "    (translational_acceleration_in_I_vec - B_cm.a1pt_theory(I0, I, P)).simplify().to_matrix(NED).subs(planetodetic_pos_derivs) + V_NED_dot_vec.to_matrix(NED)\n",
    ").reshape(3).subs(omega_p*t, 0)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 21,
   "metadata": {},
   "outputs": [],
   "source": [
    "trim_residual_extras = {}\n",
    "trim_residual_extras[sp.Array((lamda_D, phi_D, h_D))] = pcf2pd\n",
    "trim_residual_extras[g] = gravity_func\n",
    "\n",
    "V_NED_cse_no_time = sp.cse(V_NED_expr.subs(omega_p*t, 0), symbols=sp.numbered_symbols(prefix='V_NED_'), order='none')\n",
    "trim_residual_extras.update({k: v for k,v in V_NED_cse_no_time[0]})\n",
    "trim_residual_extras.update({k: v for k, v in zip(V_NED, V_NED_cse_no_time[1][0])})\n",
    "trim_residual_extras.update(navigation_cse_assignments)\n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# Kinematic codegen"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 22,
   "metadata": {},
   "outputs": [],
   "source": [
    "transformation_func_to_print = [\n",
    "    dict(num_name='inertial_to_body_dcm', input_args=q, \n",
    "        sym_expr=sp.Array(B.dcm(I).simplify()),\n",
    "        extra_assignments={}, ),\n",
    "\n",
    "]\n",
    "\n",
    "transformation_printer = codegen.ModuleNumPyPrinter(\n",
    "    for_class=False, \n",
    ")\n",
    "\n",
    "transformation_evaluator = codegen.ModulePrinter(transformation_printer)\n",
    "\n",
    "transformation_print_params = codegen.process_funcs_to_print(transformation_func_to_print)\n",
    "\n",
    "transformation_code = '\\n'.join(transformation_evaluator.codeprint(transformation_print_params).split('\\n')[2:])"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 23,
   "metadata": {},
   "outputs": [],
   "source": [
    "kin_funcs_to_print = [    \n",
    "    dict(num_name='kinematics_state_function', input_args=sp.Array([t, *kinematics_state, *kinematics_input]), \n",
    "        sym_expr=kinematics_state.diff(t).subs(kinematics_state_equation_dict),\n",
    "        extra_assignments=kinematics_state_extra_dict ),\n",
    "        \n",
    "    dict(num_name='ic_from_planetodetic', input_args=sp.Array([*ic_from_pd_args]), \n",
    "        sym_expr=ic_from_pd,\n",
    "        extra_assignments=ic_extra, ),\n",
    "    \n",
    "    dict(num_name='kinematics_output_function', input_args=sp.Array([t, *kinematics_state]), \n",
    "        sym_expr=kinematics_output.subs(kinematics_output_dict),\n",
    "        extra_assignments=kinematics_output_equation_extra_assinments_dict, ),\n",
    "    \n",
    "    dict(num_name='inertial_to_NED_dcm', input_args=sp.Array([t, lamda_D, phi_D]), \n",
    "        sym_expr=sp.Array(NED.dcm(I).simplify()),\n",
    "        extra_assignments={}, ),\n",
    "    \n",
    "    dict(num_name='local_translational_trim_residual', input_args=sp.Array([*kinematics_state[:-3], *kinematics_input[:3]]), \n",
    "        sym_expr=V_NED_dot_residual,\n",
    "        extra_assignments=trim_residual_extras, ),\n",
    "    \n",
    "    \n",
    "]\n",
    "\n",
    "static_kinematics_function_modules = {}\n",
    "for key in kinematics_function_modules:\n",
    "    static_kinematics_function_modules[key.func] = kinematics_function_modules[key]\n",
    "\n",
    "constant_modules = {omega_p: 'self', a: 'self', f: 'self'}\n",
    "constant_names= {omega_p: 'planetodetics.'+str(omega_p), a: 'planetodetics.'+str(a), f: 'planetodetics.'+str(f)}\n",
    "\n",
    "kin_code_print_params = codegen.process_funcs_to_print(kin_funcs_to_print)\n",
    "kin_printer = codegen.ModuleNumPyPrinter(\n",
    "    for_class=True,\n",
    "    function_modules=static_kinematics_function_modules,# function_names=func_names,\n",
    "    constant_modules=constant_modules, \n",
    "    constant_names = constant_names,\n",
    ")\n",
    "kin_evaluator = codegen.ModulePrinter(kin_printer)\n",
    "kin_code = kin_evaluator.codeprint(\n",
    "    (kin_code_print_params)\n",
    ")\n",
    "with open('kinematics.py', \"w\") as open_file:\n",
    "    open_file.write(kin_code)\n",
    "    open_file.write(transformation_code)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# Dynamics Modeling\n",
    "\n",
    "\n",
    "Much of the effort around the `Vehicle` design is to provide sufficient API \"hooks\" to facilitate modeling of different vehicles without re-computing (or even re-`connect`ing, in the context of SimuPy) a cumbersome number of variables. To that end, we provide hooks for extra forces and moments directly as well as extra aerodynamic coefficients. Since the mathematical modeling is simpler but with several decompositions for the purposes of API, this section has fewer distinct sections and more and more explicitly named variables.\n",
    "\n",
    "## Constants, Atmospheric flight condition, and other setup\n",
    "\n",
    "Here we define the constants used in the inertia and aerodynamic models. We also label and define atmospheric quantities used for the aerodynamics modeling. The dynamic pressure $\\bar{q}$ and Mach number $M$ could be calculated in the Kinematics model, however since Reynolds number $R_e$ also depends on the density and a vehicle-specific parameter, I kept these functions of the flight condition as part of the Dynamics model.\n",
    "\n",
    "The dynamics model expression dictionary is `dynamics_output_dict` and the extra-assignments dictionary is `dynamics_output_extra_dict`."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 24,
   "metadata": {},
   "outputs": [],
   "source": [
    "dynamics_output_dict = {}\n",
    "dynamics_output_extra_dict = {}\n",
    "\n",
    "# Inertia symbols\n",
    "m = symbols('m') \n",
    "Ixx, Iyy, Izz, Ixy, Iyz, Ixz = inertia_symbols = \\\n",
    "    symbols('I_xx, I_yy, I_zz, I_xy, I_yz, I_xz')\n",
    "com_x, com_y, com_z = com_pos = sp.Array(sp.symbols('x_com y_com z_com'))\n",
    "\n",
    "# Aerodynamic parameters\n",
    "mrc_x, mrc_y, mrc_z = mrc_pos = sp.Array(sp.symbols('x_mrc y_mrc z_mrc'))\n",
    "S_A, a_l, b_l, c_l, d_l = sp.symbols('S_A a_l b_l c_l d_l') # reference area; reference directional, lateral, longitudinal lengths; Reynolds number reference length\n",
    "ref_lengths = np.array([a_l, c_l, b_l])\n",
    "\n",
    "inertia_constants = (m, \n",
    "                   Ixx, Iyy, Izz, Ixy, Iyz, Ixz,\n",
    "                   com_x, com_y, com_z)\n",
    "aerodynamics_constants =  (mrc_x, mrc_y, mrc_z,\n",
    "                   S_A, a_l, b_l, c_l, d_l)\n",
    "dynamics_constants = (*inertia_constants, *aerodynamics_constants)\n",
    "\n",
    "# atmospheric parameters\n",
    "qbar, mach, reynolds = sp.symbols('qbar Ma Re')\n",
    "\n",
    "qbar_expr = V_T**2*density/2\n",
    "mach_expr = V_T/speed_of_sound\n",
    "Re_expr = V_T*d_l*density/viscosity\n",
    "\n",
    "dynamics_output_extra_dict[qbar] = qbar_expr\n",
    "dynamics_output_extra_dict[mach] = mach_expr\n",
    "dynamics_output_extra_dict[reynolds] = Re_expr\n",
    "\n",
    "starargs = symbols('*args')\n",
    "\n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Defining aerodynamic coefficients and their decompositions"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 25,
   "metadata": {},
   "outputs": [],
   "source": [
    "# Wind frame, freestream wind in -W.x direction -- potentially \n",
    "W = vector.frame.ReferenceFrame('W')\n",
    "# Stability frame, Sf.x and Sf.z in same plane as B.x, B.z\n",
    "Sf = W.orientnew('Sf', 'Body', [-beta, 0, 0], '312')\n",
    "Bw = Sf.orientnew('Bw', 'Body', [alpha, 0, 0], '231') # Body frame relative to wind\n",
    "\n",
    "aero_forces = sp.Array(dynamicsymbols('F_ax F_ay F_az')) # aerodynamic forces in body-fixed coordinate system\n",
    "Lcal, Mcal, Ncal = aero_moments = sp.Array(dynamicsymbols('Lcal Mcal Ncal')) # aerodynamic Roll, Pitch, Yaw moments (about body-fixed coordiante system)\n",
    "\n",
    "# total aero coefficients\n",
    "CD, CS, CL = total_aero_force_coeffs = sp.Array(dynamicsymbols('CD CS CL')) # static aero forces\n",
    "CLcal, CMcal, CNcal = total_aero_moment_coeffs = sp.Array(dynamicsymbols('CLcal CMcal CNcal')) # static aero moments\n",
    "# CLcal_s, CMcal_s, CNcal_s = transformed_static_aero_moment_coeffs = sp.Array(dynamicsymbols('CLcal_s CMcal_s CNcal_s')) # static aero moments\n",
    "Cp, Cq, Cr = total_aero_damping_coeffs = sp.Array(dynamicsymbols('Cp Cq Cr')) # dynamic aero moments\n",
    "\n",
    "# base aero coefficients\n",
    "CDb, CSb, CLb = base_aero_force_coeffs =  np.array(dynamicsymbols('CD_b CS_b CL_b'))\n",
    "CLcalb, CMcalb, CNcalb = base_aero_moment_coeffs = np.array(dynamicsymbols('CLcal_b CMcal_b CNcal_b'))\n",
    "Cpb, Cqb, Crb = base_aero_damping_coeffs = np.array(dynamicsymbols('Cp_b Cq_b Cr_b'))\n",
    "\n",
    "base_aero_coeffs = sp.Array([\n",
    "    *base_aero_force_coeffs,\n",
    "    *base_aero_moment_coeffs,\n",
    "    *base_aero_damping_coeffs,\n",
    "])\n",
    "\n",
    "\n",
    "# extra aero coefficients -- inputs, can be used to model aerodynamic control surfaces or do configuration management on aero (i.e., add wings or tail to base vehicle)\n",
    "# assumed to use same references areas/lengths as base, and moments are about same MRC\n",
    "CDe, CSe, CLe = input_aero_force_coeffs = np.array(dynamicsymbols('CD_e CS_e CL_e'))\n",
    "CLcale, CMcale, CNcale = input_aero_moment_coeffs = np.array(dynamicsymbols('CLcal_e CMcal_e CNcal_e'))\n",
    "Cpe, Cqe, Cre = input_aero_damping_coeffs = np.array(dynamicsymbols('Cp_e Cq_e Cr_e'))\n",
    "\n",
    "\n",
    "input_aero_coeffs = sp.Array([\n",
    "    *input_aero_force_coeffs,\n",
    "    *input_aero_moment_coeffs,\n",
    "    *input_aero_damping_coeffs,\n",
    "])\n",
    "\n",
    "\n",
    "\n",
    "\n",
    "total_aero_force_coeffs = base_aero_force_coeffs + input_aero_force_coeffs\n",
    "total_aero_damping_coeffs_mrc = base_aero_damping_coeffs + input_aero_damping_coeffs\n",
    "total_static_aero_moment_coeffs_mrc = base_aero_moment_coeffs + input_aero_moment_coeffs \n",
    "total_aero_moment_coeffs_mrc = total_static_aero_moment_coeffs_mrc + total_aero_damping_coeffs_mrc*ref_lengths*NED_ang_vel_B*sp.Piecewise((1/(2*V_T), V_T> 0.), (0., True))"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Total Aerodynamic Forces and Moments\n",
    "\n",
    "For the aerodynamic forces, we must transform the wind-frame Lift, Drag, and Side-forces to body-fixed Normal, Axial, and Side-forces. Note that these two side-forces do not point in the same direction for non-zero sideslip.\n",
    "\n",
    "For the aerodynamic moments, we assume the moment coefficients are about a moment reference frame (MRC) so the moment coefficients must be transformed to the center of mass position. Note that we apply the same transformation to the damping coefficients."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 26,
   "metadata": {},
   "outputs": [],
   "source": [
    "body_unit_vecs = np.array([Bw.x, Bw.y, Bw.z])\n",
    "wind_unit_vecs = np.array([W.x, W.y, W.z])\n",
    "\n",
    "\n",
    "# force transformation\n",
    "total_aero_force_coeffs_vec = np.sum(np.array([-1, 1, -1])*total_aero_force_coeffs*wind_unit_vecs)\n",
    "total_aero_forces_expr = qbar*S_A*np.array(total_aero_force_coeffs_vec.to_matrix(Bw)).squeeze()\n",
    "\n",
    "# moment transformation\n",
    "total_aero_moment_coeffs_mrc_vec = np.sum(total_aero_moment_coeffs_mrc*body_unit_vecs)\n",
    "mrc_to_com = np.array(com_pos - mrc_pos)\n",
    "mrc_to_com_vec = np.sum(mrc_to_com*body_unit_vecs)\n",
    "total_aero_moment_coeffs_com = np.array(total_aero_moment_coeffs_mrc - np.array((total_aero_force_coeffs_vec.cross(mrc_to_com_vec)).to_matrix(Bw)).squeeze()/ref_lengths)\n",
    "total_aero_moments_com_expr = qbar*S_A*total_aero_moment_coeffs_com*ref_lengths\n",
    "\n",
    "\n",
    "# in general, moment coefficient = static coefficient + ((dynamic coefficient * angular rate * ref length) / (2 * true airspeed))\n",
    "# LaRC slides show an extra half factor on dynamic term, but NESC description doesn't have that... coefficients can be re-normalized\n",
    "# so just need to clarify the definition\n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Preparing for Codegen"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 27,
   "metadata": {},
   "outputs": [],
   "source": [
    "# \n",
    "total_aero_forces_moments_syms = sp.Array([*aero_forces, *aero_moments])\n",
    "total_aero_forces_moments_expr = sp.Array([*total_aero_forces_expr, *total_aero_moments_com_expr])\n",
    "\n",
    "\n",
    "\n",
    "total_aero_forces_moments_func = sp.Function('tot_aero_forces_moments')(qbar, mach, reynolds, V_T, alpha, beta, *NED_ang_vel_B, starargs)\n",
    "total_aero_forces_moments_func.shape = (6,)\n",
    "dynamics_output_extra_dict[total_aero_forces_moments_syms] = total_aero_forces_moments_func\n",
    "tot_aero_extra_dict = {}\n",
    "\n",
    "base_aero_coeffs_func = sp.Function('base_aero_coeffs')(alpha, beta, mach, reynolds)\n",
    "base_aero_coeffs_func.shape = (9,)\n",
    "tot_aero_extra_dict[base_aero_coeffs] = base_aero_coeffs_func\n",
    "\n",
    "input_aero_coeffs_func = sp.Function('_input_aero_coeffs')(alpha, beta, mach, reynolds, starargs)\n",
    "input_aero_coeffs_func.shape = (9,)\n",
    "tot_aero_extra_dict[input_aero_coeffs] = input_aero_coeffs_func\n",
    "\n",
    "# input forces and torques/moments in body-fixed coordinates (moments about center of mass)\n",
    "Phix, Phiy, Phiz = input_forces = sp.Array(dynamicsymbols('Phi_x Phi_y Phi_z'))\n",
    "taux, tauy, tauz = input_moments = sp.Array(dynamicsymbols('tau_x tau_y tau_z'))\n",
    "input_forces_moments = sp.Array([*input_forces, *input_moments])\n",
    "\n",
    "input_forces_moments_func = sp.Function('_input_force_moment')(t, *kinematics_output, qbar, mach, reynolds, starargs)\n",
    "input_forces_moments_func.shape = (6,)\n",
    "dynamics_output_extra_dict[input_forces_moments] = input_forces_moments_func\n",
    "\n",
    "\n",
    "# total forces and moments\n",
    "Fx, Fy, Fz = tot_body_fixed_forces_var = sp.Array(dynamicsymbols('F_x F_y F_z')) # total body-fixed forces\n",
    "Mx, My, Mz = tot_body_fixed_moments_var = sp.Array(dynamicsymbols('M_x M_y M_z')) # total body-fixed moments\n",
    "\n",
    "\n",
    "\n",
    "tot_forces_moments_syms = sp.Array([*tot_body_fixed_forces_var, *tot_body_fixed_moments_var])\n",
    "tot_forces_moments_expr = total_aero_forces_moments_syms + input_forces_moments\n",
    "dynamics_output_extra_dict[tot_forces_moments_syms] = tot_forces_moments_expr\n",
    "\n",
    "\n",
    "\n",
    "dynamics_input = sp.Array([\n",
    "    t,\n",
    "    *kinematics_output,\n",
    "    starargs,\n",
    "])\n",
    "\n",
    "dynamics_output = sp.Array([*inertial_acc_B, *I_ang_acc_B])\n",
    "# TODO: make the angular and translational accelerations independent hooked functions that take (Mx My Mz) and (Fx Fy Fz), respectively."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# Dynamics output\n",
    "\n",
    "The translational acceleration of the vehicle center of mass and the angular acceleration of the vehicle body about the center of mass is computed using the Newton-Euler equation. For translational acceleration, we simply sum the forces projected in the body-fixed axes and divide by the (assumed constant) mass $m$ as shown below. The angular acceleration for a constant inertia $\\mathbb{I}$ is given by \n",
    "\n",
    "$$\\left[\\begin{matrix}\\dot{\\omega}_{x}\\\\\n",
    "\\dot{\\omega}_{y}\\\\\n",
    "\\dot{\\omega}_{z}\n",
    "\\end{matrix}\\right]=\\mathbb{I}^{-1}\\left(\\left[\\begin{matrix}M_{x}\\\\\n",
    "M_{y}\\\\\n",
    "M_{z}\n",
    "\\end{matrix}\\right]-\\left[\\begin{matrix}\\omega_{x}\\\\\n",
    "\\omega_{y}\\\\\n",
    "\\omega_{z}\n",
    "\\end{matrix}\\right]\\times\\mathbb{I}\\cdot\\left[\\begin{matrix}\\omega_{x}\\\\\n",
    "\\omega_{y}\\\\\n",
    "\\omega_{z}\n",
    "\\end{matrix}\\right]\\right)$$\n",
    "\n",
    "Below, we see the operations of this vector-matrix notation carried out."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 28,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/latex": [
       "$\\displaystyle \\operatorname{A_{X}}{\\left(t \\right)} = \\frac{\\operatorname{F_{x}}{\\left(t \\right)}}{m}$"
      ],
      "text/plain": [
       "Eq(A_X(t), F_x(t)/m)"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    },
    {
     "data": {
      "text/latex": [
       "$\\displaystyle \\operatorname{A_{Y}}{\\left(t \\right)} = \\frac{\\operatorname{F_{y}}{\\left(t \\right)}}{m}$"
      ],
      "text/plain": [
       "Eq(A_Y(t), F_y(t)/m)"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    },
    {
     "data": {
      "text/latex": [
       "$\\displaystyle \\operatorname{A_{Z}}{\\left(t \\right)} = \\frac{\\operatorname{F_{z}}{\\left(t \\right)}}{m}$"
      ],
      "text/plain": [
       "Eq(A_Z(t), F_z(t)/m)"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    }
   ],
   "source": [
    "inertial_acc_B_dict = sp.solve(m*inertial_acc_B - tot_body_fixed_forces_var, inertial_acc_B) # this assumes constant mass\n",
    "dynamics_output_dict.update(inertial_acc_B_dict)\n",
    "display_dict(inertial_acc_B_dict)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 29,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/latex": [
       "$\\displaystyle \\alpha_{X}{\\left(t \\right)} = - \\frac{\\left(I_{xy} I_{yz} + I_{xz} I_{yy}\\right) \\left(I_{xx} \\omega_{X}{\\left(t \\right)} \\omega_{Y}{\\left(t \\right)} + I_{xy} \\omega_{X}^{2}{\\left(t \\right)} - I_{xy} \\omega_{Y}^{2}{\\left(t \\right)} - I_{xz} \\omega_{Y}{\\left(t \\right)} \\omega_{Z}{\\left(t \\right)} - I_{yy} \\omega_{X}{\\left(t \\right)} \\omega_{Y}{\\left(t \\right)} + I_{yz} \\omega_{X}{\\left(t \\right)} \\omega_{Z}{\\left(t \\right)} + \\operatorname{M_{z}}{\\left(t \\right)}\\right) + \\left(I_{xy} I_{zz} + I_{xz} I_{yz}\\right) \\left(- I_{xx} \\omega_{X}{\\left(t \\right)} \\omega_{Z}{\\left(t \\right)} + I_{xy} \\omega_{Y}{\\left(t \\right)} \\omega_{Z}{\\left(t \\right)} - I_{xz} \\omega_{X}^{2}{\\left(t \\right)} + I_{xz} \\omega_{Z}^{2}{\\left(t \\right)} - I_{yz} \\omega_{X}{\\left(t \\right)} \\omega_{Y}{\\left(t \\right)} + I_{zz} \\omega_{X}{\\left(t \\right)} \\omega_{Z}{\\left(t \\right)} + \\operatorname{M_{y}}{\\left(t \\right)}\\right) + \\left(I_{yy} I_{zz} - I_{yz}^{2}\\right) \\left(- I_{xy} \\omega_{X}{\\left(t \\right)} \\omega_{Z}{\\left(t \\right)} + I_{xz} \\omega_{X}{\\left(t \\right)} \\omega_{Y}{\\left(t \\right)} + I_{yy} \\omega_{Y}{\\left(t \\right)} \\omega_{Z}{\\left(t \\right)} + I_{yz} \\omega_{Y}^{2}{\\left(t \\right)} - I_{yz} \\omega_{Z}^{2}{\\left(t \\right)} - I_{zz} \\omega_{Y}{\\left(t \\right)} \\omega_{Z}{\\left(t \\right)} + \\operatorname{M_{x}}{\\left(t \\right)}\\right)}{- I_{xx} I_{yy} I_{zz} + I_{xx} I_{yz}^{2} + I_{xy}^{2} I_{zz} + 2 I_{xy} I_{xz} I_{yz} + I_{xz}^{2} I_{yy}}$"
      ],
      "text/plain": [
       "Eq(alpha_X(t), -((I_xy*I_yz + I_xz*I_yy)*(I_xx*omega_X(t)*omega_Y(t) + I_xy*omega_X(t)**2 - I_xy*omega_Y(t)**2 - I_xz*omega_Y(t)*omega_Z(t) - I_yy*omega_X(t)*omega_Y(t) + I_yz*omega_X(t)*omega_Z(t) + M_z(t)) + (I_xy*I_zz + I_xz*I_yz)*(-I_xx*omega_X(t)*omega_Z(t) + I_xy*omega_Y(t)*omega_Z(t) - I_xz*omega_X(t)**2 + I_xz*omega_Z(t)**2 - I_yz*omega_X(t)*omega_Y(t) + I_zz*omega_X(t)*omega_Z(t) + M_y(t)) + (I_yy*I_zz - I_yz**2)*(-I_xy*omega_X(t)*omega_Z(t) + I_xz*omega_X(t)*omega_Y(t) + I_yy*omega_Y(t)*omega_Z(t) + I_yz*omega_Y(t)**2 - I_yz*omega_Z(t)**2 - I_zz*omega_Y(t)*omega_Z(t) + M_x(t)))/(-I_xx*I_yy*I_zz + I_xx*I_yz**2 + I_xy**2*I_zz + 2*I_xy*I_xz*I_yz + I_xz**2*I_yy))"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    },
    {
     "data": {
      "text/latex": [
       "$\\displaystyle \\alpha_{Y}{\\left(t \\right)} = - \\frac{\\left(I_{xx} I_{yz} + I_{xy} I_{xz}\\right) \\left(I_{xx} \\omega_{X}{\\left(t \\right)} \\omega_{Y}{\\left(t \\right)} + I_{xy} \\omega_{X}^{2}{\\left(t \\right)} - I_{xy} \\omega_{Y}^{2}{\\left(t \\right)} - I_{xz} \\omega_{Y}{\\left(t \\right)} \\omega_{Z}{\\left(t \\right)} - I_{yy} \\omega_{X}{\\left(t \\right)} \\omega_{Y}{\\left(t \\right)} + I_{yz} \\omega_{X}{\\left(t \\right)} \\omega_{Z}{\\left(t \\right)} + \\operatorname{M_{z}}{\\left(t \\right)}\\right) + \\left(I_{xx} I_{zz} - I_{xz}^{2}\\right) \\left(- I_{xx} \\omega_{X}{\\left(t \\right)} \\omega_{Z}{\\left(t \\right)} + I_{xy} \\omega_{Y}{\\left(t \\right)} \\omega_{Z}{\\left(t \\right)} - I_{xz} \\omega_{X}^{2}{\\left(t \\right)} + I_{xz} \\omega_{Z}^{2}{\\left(t \\right)} - I_{yz} \\omega_{X}{\\left(t \\right)} \\omega_{Y}{\\left(t \\right)} + I_{zz} \\omega_{X}{\\left(t \\right)} \\omega_{Z}{\\left(t \\right)} + \\operatorname{M_{y}}{\\left(t \\right)}\\right) + \\left(I_{xy} I_{zz} + I_{xz} I_{yz}\\right) \\left(- I_{xy} \\omega_{X}{\\left(t \\right)} \\omega_{Z}{\\left(t \\right)} + I_{xz} \\omega_{X}{\\left(t \\right)} \\omega_{Y}{\\left(t \\right)} + I_{yy} \\omega_{Y}{\\left(t \\right)} \\omega_{Z}{\\left(t \\right)} + I_{yz} \\omega_{Y}^{2}{\\left(t \\right)} - I_{yz} \\omega_{Z}^{2}{\\left(t \\right)} - I_{zz} \\omega_{Y}{\\left(t \\right)} \\omega_{Z}{\\left(t \\right)} + \\operatorname{M_{x}}{\\left(t \\right)}\\right)}{- I_{xx} I_{yy} I_{zz} + I_{xx} I_{yz}^{2} + I_{xy}^{2} I_{zz} + 2 I_{xy} I_{xz} I_{yz} + I_{xz}^{2} I_{yy}}$"
      ],
      "text/plain": [
       "Eq(alpha_Y(t), -((I_xx*I_yz + I_xy*I_xz)*(I_xx*omega_X(t)*omega_Y(t) + I_xy*omega_X(t)**2 - I_xy*omega_Y(t)**2 - I_xz*omega_Y(t)*omega_Z(t) - I_yy*omega_X(t)*omega_Y(t) + I_yz*omega_X(t)*omega_Z(t) + M_z(t)) + (I_xx*I_zz - I_xz**2)*(-I_xx*omega_X(t)*omega_Z(t) + I_xy*omega_Y(t)*omega_Z(t) - I_xz*omega_X(t)**2 + I_xz*omega_Z(t)**2 - I_yz*omega_X(t)*omega_Y(t) + I_zz*omega_X(t)*omega_Z(t) + M_y(t)) + (I_xy*I_zz + I_xz*I_yz)*(-I_xy*omega_X(t)*omega_Z(t) + I_xz*omega_X(t)*omega_Y(t) + I_yy*omega_Y(t)*omega_Z(t) + I_yz*omega_Y(t)**2 - I_yz*omega_Z(t)**2 - I_zz*omega_Y(t)*omega_Z(t) + M_x(t)))/(-I_xx*I_yy*I_zz + I_xx*I_yz**2 + I_xy**2*I_zz + 2*I_xy*I_xz*I_yz + I_xz**2*I_yy))"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    },
    {
     "data": {
      "text/latex": [
       "$\\displaystyle \\alpha_{Z}{\\left(t \\right)} = - \\frac{\\left(I_{xx} I_{yy} - I_{xy}^{2}\\right) \\left(I_{xx} \\omega_{X}{\\left(t \\right)} \\omega_{Y}{\\left(t \\right)} + I_{xy} \\omega_{X}^{2}{\\left(t \\right)} - I_{xy} \\omega_{Y}^{2}{\\left(t \\right)} - I_{xz} \\omega_{Y}{\\left(t \\right)} \\omega_{Z}{\\left(t \\right)} - I_{yy} \\omega_{X}{\\left(t \\right)} \\omega_{Y}{\\left(t \\right)} + I_{yz} \\omega_{X}{\\left(t \\right)} \\omega_{Z}{\\left(t \\right)} + \\operatorname{M_{z}}{\\left(t \\right)}\\right) + \\left(I_{xx} I_{yz} + I_{xy} I_{xz}\\right) \\left(- I_{xx} \\omega_{X}{\\left(t \\right)} \\omega_{Z}{\\left(t \\right)} + I_{xy} \\omega_{Y}{\\left(t \\right)} \\omega_{Z}{\\left(t \\right)} - I_{xz} \\omega_{X}^{2}{\\left(t \\right)} + I_{xz} \\omega_{Z}^{2}{\\left(t \\right)} - I_{yz} \\omega_{X}{\\left(t \\right)} \\omega_{Y}{\\left(t \\right)} + I_{zz} \\omega_{X}{\\left(t \\right)} \\omega_{Z}{\\left(t \\right)} + \\operatorname{M_{y}}{\\left(t \\right)}\\right) + \\left(I_{xy} I_{yz} + I_{xz} I_{yy}\\right) \\left(- I_{xy} \\omega_{X}{\\left(t \\right)} \\omega_{Z}{\\left(t \\right)} + I_{xz} \\omega_{X}{\\left(t \\right)} \\omega_{Y}{\\left(t \\right)} + I_{yy} \\omega_{Y}{\\left(t \\right)} \\omega_{Z}{\\left(t \\right)} + I_{yz} \\omega_{Y}^{2}{\\left(t \\right)} - I_{yz} \\omega_{Z}^{2}{\\left(t \\right)} - I_{zz} \\omega_{Y}{\\left(t \\right)} \\omega_{Z}{\\left(t \\right)} + \\operatorname{M_{x}}{\\left(t \\right)}\\right)}{- I_{xx} I_{yy} I_{zz} + I_{xx} I_{yz}^{2} + I_{xy}^{2} I_{zz} + 2 I_{xy} I_{xz} I_{yz} + I_{xz}^{2} I_{yy}}$"
      ],
      "text/plain": [
       "Eq(alpha_Z(t), -((I_xx*I_yy - I_xy**2)*(I_xx*omega_X(t)*omega_Y(t) + I_xy*omega_X(t)**2 - I_xy*omega_Y(t)**2 - I_xz*omega_Y(t)*omega_Z(t) - I_yy*omega_X(t)*omega_Y(t) + I_yz*omega_X(t)*omega_Z(t) + M_z(t)) + (I_xx*I_yz + I_xy*I_xz)*(-I_xx*omega_X(t)*omega_Z(t) + I_xy*omega_Y(t)*omega_Z(t) - I_xz*omega_X(t)**2 + I_xz*omega_Z(t)**2 - I_yz*omega_X(t)*omega_Y(t) + I_zz*omega_X(t)*omega_Z(t) + M_y(t)) + (I_xy*I_yz + I_xz*I_yy)*(-I_xy*omega_X(t)*omega_Z(t) + I_xz*omega_X(t)*omega_Y(t) + I_yy*omega_Y(t)*omega_Z(t) + I_yz*omega_Y(t)**2 - I_yz*omega_Z(t)**2 - I_zz*omega_Y(t)*omega_Z(t) + M_x(t)))/(-I_xx*I_yy*I_zz + I_xx*I_yz**2 + I_xy**2*I_zz + 2*I_xy*I_xz*I_yz + I_xz**2*I_yy))"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    }
   ],
   "source": [
    "II = inertia(Bw, Ixx, Iyy, Izz, -Ixy, -Iyz, -Ixz)\n",
    "Bw_ang_vel_I = omega_X*Bw.x + omega_Y*Bw.y + omega_Z*Bw.z\n",
    "Bw_ang_acc_I = alpha_X*Bw.x + alpha_Y*Bw.y + alpha_Z*Bw.z\n",
    "inertial_ang_acc_B_dict = sp.solve(sp.Array((II.dot(Bw_ang_acc_I) + Bw_ang_vel_I.cross(II.dot(Bw_ang_vel_I))).to_matrix(Bw))[:, 0] - tot_body_fixed_moments_var, I_ang_acc_B)\n",
    "dynamics_output_dict.update(inertial_ang_acc_B_dict)\n",
    "\n",
    "display_dict(inertial_ang_acc_B_dict)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 30,
   "metadata": {},
   "outputs": [],
   "source": [
    "\n",
    "\n",
    "transformation_func_to_print = [\n",
    "    dict(num_name='body_to_wind_dcm', input_args=sp.Array([alpha, beta]), \n",
    "        sym_expr=sp.Array(W.dcm(Bw)),\n",
    "        extra_assignments={}, ),\n",
    "    \n",
    "]\n",
    "\n",
    "transformation_printer = codegen.ModuleNumPyPrinter(\n",
    "    for_class=False, \n",
    ")\n",
    "\n",
    "transformation_evaluator = codegen.ModulePrinter(transformation_printer)\n",
    "\n",
    "transformation_print_params = codegen.process_funcs_to_print(transformation_func_to_print)\n",
    "\n",
    "transformation_code = '\\n'.join(transformation_evaluator.codeprint(transformation_print_params).split('\\n')[2:])\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 31,
   "metadata": {},
   "outputs": [],
   "source": [
    "def CPM(vec):\n",
    "    return sp.Array(\n",
    "        [\n",
    "            [0, -vec[2], vec[1],],\n",
    "            [vec[2], 0, -vec[0],],\n",
    "            [-vec[1], vec[0], 0,],\n",
    "        ]\n",
    "    )"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 32,
   "metadata": {},
   "outputs": [],
   "source": [
    "dyn_funcs_to_print = [\n",
    "    dict(num_name=total_aero_forces_moments_func.func.__name__, input_args=sp.Array(total_aero_forces_moments_func.args), \n",
    "        sym_expr=total_aero_forces_moments_expr,\n",
    "        extra_assignments=tot_aero_extra_dict, ),\n",
    "    \n",
    "    dict(num_name='dynamics_output_function', input_args=dynamics_input, \n",
    "        sym_expr=dynamics_output.subs(dynamics_output_dict),\n",
    "        extra_assignments=dynamics_output_extra_dict, ),\n",
    "    \n",
    "    \n",
    "    dict(num_name='mrc_to_com_cpm', input_args=None, \n",
    "        sym_expr=CPM(mrc_to_com_vec.to_matrix(Bw)),\n",
    "        extra_assignments={}, ),\n",
    "\n",
    "]\n",
    "\n",
    "dyn_printer = codegen.ModuleNumPyPrinter(\n",
    "    for_class=True, function_modules={total_aero_forces_moments_func.func: 'self', base_aero_coeffs_func.func: 'self',},\n",
    "#     function_modules=func_modules, function_names=func_names,\n",
    "    constant_modules={var: 'self' for var in dynamics_constants}\n",
    ")\n",
    "dyn_evaluator = codegen.ModulePrinter(dyn_printer)\n",
    "\n",
    "dyn_code_print_params = codegen.process_funcs_to_print(dyn_funcs_to_print)\n",
    "\n",
    "dyn_code = dyn_evaluator.codeprint(dyn_code_print_params) \n",
    "\n",
    "with open('dynamics.py', \"w\") as open_file:\n",
    "    open_file.write(dyn_code)\n",
    "    open_file.write(transformation_code)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Here we print plain-text column names, LaTeX column names, and a set of index constants to facilitate user development with the SimuPy Flight Vehicle Toolkit blocks."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 33,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "['p_x', 'p_y', 'p_z', 'q_0', 'q_1', 'q_2', 'q_3', 'v_x', 'v_y', 'v_z', 'omega_X', 'omega_Y', 'omega_Z', 'lamda_D', 'phi_D', 'h_D', 'psi', 'theta', 'phi', 'rho', 'c_s', 'mu', 'V_T', 'alpha', 'beta', 'p_B', 'q_B', 'r_B', 'V_N', 'V_E', 'V_D', 'W_N', 'W_E', 'W_D', 'A_X', 'A_Y', 'A_Z', 'alpha_X', 'alpha_Y', 'alpha_Z'] \n",
      "\n",
      "['$p_{x}$', '$p_{y}$', '$p_{z}$', '$q_{0}$', '$q_{1}$', '$q_{2}$', '$q_{3}$', '$v_{x}$', '$v_{y}$', '$v_{z}$', '$\\\\omega_{X}$', '$\\\\omega_{Y}$', '$\\\\omega_{Z}$', '$\\\\lambda_{D}$', '$\\\\phi_{D}$', '$h_{D}$', '$\\\\psi$', '$\\\\theta$', '$\\\\phi$', '$\\\\rho$', '$c_{s}$', '$\\\\mu$', '$V_{T}$', '$\\\\alpha$', '$\\\\beta$', '$p_{B}$', '$q_{B}$', '$r_{B}$', '$V_{N}$', '$V_{E}$', '$V_{D}$', '$W_{N}$', '$W_{E}$', '$W_{D}$', '$A_{X}$', '$A_{Y}$', '$A_{Z}$', '$\\\\alpha_{X}$', '$\\\\alpha_{Y}$', '$\\\\alpha_{Z}$'] \n",
      "\n",
      "p_x_idx = 0\n",
      "p_y_idx = 1\n",
      "p_z_idx = 2\n",
      "q_0_idx = 3\n",
      "q_1_idx = 4\n",
      "q_2_idx = 5\n",
      "q_3_idx = 6\n",
      "v_x_idx = 7\n",
      "v_y_idx = 8\n",
      "v_z_idx = 9\n",
      "omega_X_idx = 10\n",
      "omega_Y_idx = 11\n",
      "omega_Z_idx = 12\n",
      "lamda_D_idx = 13\n",
      "phi_D_idx = 14\n",
      "h_D_idx = 15\n",
      "psi_idx = 16\n",
      "theta_idx = 17\n",
      "phi_idx = 18\n",
      "rho_idx = 19\n",
      "c_s_idx = 20\n",
      "mu_idx = 21\n",
      "V_T_idx = 22\n",
      "alpha_idx = 23\n",
      "beta_idx = 24\n",
      "p_B_idx = 25\n",
      "q_B_idx = 26\n",
      "r_B_idx = 27\n",
      "V_N_idx = 28\n",
      "V_E_idx = 29\n",
      "V_D_idx = 30\n",
      "W_N_idx = 31\n",
      "W_E_idx = 32\n",
      "W_D_idx = 33\n",
      "\n",
      "\n",
      "t_arg_idx = 0\n",
      "p_x_arg_idx = 1\n",
      "p_y_arg_idx = 2\n",
      "p_z_arg_idx = 3\n",
      "q_0_arg_idx = 4\n",
      "q_1_arg_idx = 5\n",
      "q_2_arg_idx = 6\n",
      "q_3_arg_idx = 7\n",
      "v_x_arg_idx = 8\n",
      "v_y_arg_idx = 9\n",
      "v_z_arg_idx = 10\n",
      "omega_X_arg_idx = 11\n",
      "omega_Y_arg_idx = 12\n",
      "omega_Z_arg_idx = 13\n",
      "lamda_D_arg_idx = 14\n",
      "phi_D_arg_idx = 15\n",
      "h_D_arg_idx = 16\n",
      "psi_arg_idx = 17\n",
      "theta_arg_idx = 18\n",
      "phi_arg_idx = 19\n",
      "rho_arg_idx = 20\n",
      "c_s_arg_idx = 21\n",
      "mu_arg_idx = 22\n",
      "V_T_arg_idx = 23\n",
      "alpha_arg_idx = 24\n",
      "beta_arg_idx = 25\n",
      "p_B_arg_idx = 26\n",
      "q_B_arg_idx = 27\n",
      "r_B_arg_idx = 28\n",
      "V_N_arg_idx = 29\n",
      "V_E_arg_idx = 30\n",
      "V_D_arg_idx = 31\n",
      "W_N_arg_idx = 32\n",
      "W_E_arg_idx = 33\n",
      "W_D_arg_idx = 34\n",
      "qbar_arg_idx = 35\n",
      "Ma_arg_idx = 36\n",
      "Re_arg_idx = 37\n",
      "*args_arg_idx = 38\n"
     ]
    }
   ],
   "source": [
    "print([str(arg) for arg in codegen.staticfy_expressions(kinematics_output)] + [str(arg) for arg in codegen.staticfy_expressions(dynamics_output)], \"\\n\")\n",
    "print(['$'+sp.latex(arg)+'$' for arg in codegen.staticfy_expressions(kinematics_output)] + ['$'+sp.latex(arg)+'$' for arg in codegen.staticfy_expressions(dynamics_output)],\"\\n\")\n",
    "for idx, arg in enumerate(codegen.staticfy_expressions(kinematics_output)):\n",
    "    print(\"%s_idx = %d\" % (str(arg), idx))\n",
    "print(\"\\n\")\n",
    "for idx, arg in enumerate(codegen.staticfy_expressions(sp.Array(input_forces_moments_func.args))):\n",
    "    print(\"%s_arg_idx = %d\" % (str(arg), idx))"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": []
  }
 ],
 "metadata": {
  "kernelspec": {
   "display_name": "Python 3",
   "language": "python",
   "name": "python3"
  },
  "language_info": {
   "codemirror_mode": {
    "name": "ipython",
    "version": 3
   },
   "file_extension": ".py",
   "mimetype": "text/x-python",
   "name": "python",
   "nbconvert_exporter": "python",
   "pygments_lexer": "ipython3",
   "version": "3.6.8"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 4
}
